This source file includes following definitions.
- vfe_get_bpp
- vfe_find_code
- vfe_src_pad_code
- vfe_reset
- vfe_halt
- vfe_init_outputs
- vfe_reset_output_maps
- vfe_output_init_addrs
- vfe_output_update_ping_addr
- vfe_output_update_pong_addr
- vfe_reserve_wm
- vfe_release_wm
- vfe_output_frame_drop
- vfe_buf_get_pending
- vfe_buf_add_pending
- vfe_buf_flush_pending
- vfe_buf_update_wm_on_next
- vfe_buf_update_wm_on_last
- vfe_buf_update_wm_on_new
- vfe_get_output
- vfe_put_output
- vfe_enable_output
- vfe_disable_output
- vfe_enable
- vfe_disable
- vfe_isr_sof
- vfe_isr_reg_update
- vfe_isr_wm_done
- vfe_isr_comp_done
- vfe_isr_reset_ack
- vfe_isr_halt_ack
- vfe_set_clock_rates
- vfe_check_clock_rates
- vfe_get
- vfe_put
- vfe_queue_buffer
- vfe_flush_buffers
- vfe_set_power
- vfe_set_stream
- __vfe_get_format
- __vfe_get_compose
- __vfe_get_crop
- vfe_try_format
- vfe_try_compose
- vfe_try_crop
- vfe_enum_mbus_code
- vfe_enum_frame_size
- vfe_get_format
- vfe_set_format
- vfe_get_selection
- vfe_set_selection
- vfe_init_formats
- msm_vfe_subdev_init
- msm_vfe_get_vfe_id
- msm_vfe_get_vfe_line_id
- vfe_link_setup
- msm_vfe_stop_streaming
- msm_vfe_register_entities
- msm_vfe_unregister_entities
1
2
3
4
5
6
7
8
9
10 #include <linux/clk.h>
11 #include <linux/completion.h>
12 #include <linux/interrupt.h>
13 #include <linux/iommu.h>
14 #include <linux/mutex.h>
15 #include <linux/of.h>
16 #include <linux/platform_device.h>
17 #include <linux/pm_runtime.h>
18 #include <linux/spinlock_types.h>
19 #include <linux/spinlock.h>
20 #include <media/media-entity.h>
21 #include <media/v4l2-device.h>
22 #include <media/v4l2-subdev.h>
23
24 #include "camss-vfe.h"
25 #include "camss.h"
26
27 #define MSM_VFE_NAME "msm_vfe"
28
29 #define vfe_line_array(ptr_line) \
30 ((const struct vfe_line (*)[]) &(ptr_line[-(ptr_line->id)]))
31
32 #define to_vfe(ptr_line) \
33 container_of(vfe_line_array(ptr_line), struct vfe_device, line)
34
35
36 #define VFE_RESET_TIMEOUT_MS 50
37
38 #define VFE_HALT_TIMEOUT_MS 100
39
40 #define VFE_FRAME_DROP_UPDATES 2
41
42 #define VFE_FRAME_DROP_VAL 30
43
44 #define VFE_NEXT_SOF_MS 500
45
46 #define SCALER_RATIO_MAX 16
47
48 struct vfe_format {
49 u32 code;
50 u8 bpp;
51 };
52
53 static const struct vfe_format formats_rdi_8x16[] = {
54 { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
55 { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
56 { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
57 { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
58 { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
59 { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
60 { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
61 { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
62 { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
63 { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
64 { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
65 { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
66 { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
67 { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
68 { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
69 { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
70 { MEDIA_BUS_FMT_Y10_1X10, 10 },
71 };
72
73 static const struct vfe_format formats_pix_8x16[] = {
74 { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
75 { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
76 { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
77 { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
78 };
79
80 static const struct vfe_format formats_rdi_8x96[] = {
81 { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
82 { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
83 { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
84 { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
85 { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
86 { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
87 { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
88 { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
89 { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
90 { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
91 { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
92 { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
93 { MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16 },
94 { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
95 { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
96 { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
97 { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
98 { MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
99 { MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
100 { MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
101 { MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
102 { MEDIA_BUS_FMT_Y10_1X10, 10 },
103 { MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16 },
104 };
105
106 static const struct vfe_format formats_pix_8x96[] = {
107 { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
108 { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
109 { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
110 { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
111 };
112
113
114
115
116
117
118
119
120
121 static u8 vfe_get_bpp(const struct vfe_format *formats,
122 unsigned int nformats, u32 code)
123 {
124 unsigned int i;
125
126 for (i = 0; i < nformats; i++)
127 if (code == formats[i].code)
128 return formats[i].bpp;
129
130 WARN(1, "Unknown format\n");
131
132 return formats[0].bpp;
133 }
134
135 static u32 vfe_find_code(u32 *code, unsigned int n_code,
136 unsigned int index, u32 req_code)
137 {
138 int i;
139
140 if (!req_code && (index >= n_code))
141 return 0;
142
143 for (i = 0; i < n_code; i++)
144 if (req_code) {
145 if (req_code == code[i])
146 return req_code;
147 } else {
148 if (i == index)
149 return code[i];
150 }
151
152 return code[0];
153 }
154
155 static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
156 unsigned int index, u32 src_req_code)
157 {
158 struct vfe_device *vfe = to_vfe(line);
159
160 if (vfe->camss->version == CAMSS_8x16)
161 switch (sink_code) {
162 case MEDIA_BUS_FMT_YUYV8_2X8:
163 {
164 u32 src_code[] = {
165 MEDIA_BUS_FMT_YUYV8_2X8,
166 MEDIA_BUS_FMT_YUYV8_1_5X8,
167 };
168
169 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
170 index, src_req_code);
171 }
172 case MEDIA_BUS_FMT_YVYU8_2X8:
173 {
174 u32 src_code[] = {
175 MEDIA_BUS_FMT_YVYU8_2X8,
176 MEDIA_BUS_FMT_YVYU8_1_5X8,
177 };
178
179 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
180 index, src_req_code);
181 }
182 case MEDIA_BUS_FMT_UYVY8_2X8:
183 {
184 u32 src_code[] = {
185 MEDIA_BUS_FMT_UYVY8_2X8,
186 MEDIA_BUS_FMT_UYVY8_1_5X8,
187 };
188
189 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
190 index, src_req_code);
191 }
192 case MEDIA_BUS_FMT_VYUY8_2X8:
193 {
194 u32 src_code[] = {
195 MEDIA_BUS_FMT_VYUY8_2X8,
196 MEDIA_BUS_FMT_VYUY8_1_5X8,
197 };
198
199 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
200 index, src_req_code);
201 }
202 default:
203 if (index > 0)
204 return 0;
205
206 return sink_code;
207 }
208 else if (vfe->camss->version == CAMSS_8x96)
209 switch (sink_code) {
210 case MEDIA_BUS_FMT_YUYV8_2X8:
211 {
212 u32 src_code[] = {
213 MEDIA_BUS_FMT_YUYV8_2X8,
214 MEDIA_BUS_FMT_YVYU8_2X8,
215 MEDIA_BUS_FMT_UYVY8_2X8,
216 MEDIA_BUS_FMT_VYUY8_2X8,
217 MEDIA_BUS_FMT_YUYV8_1_5X8,
218 };
219
220 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
221 index, src_req_code);
222 }
223 case MEDIA_BUS_FMT_YVYU8_2X8:
224 {
225 u32 src_code[] = {
226 MEDIA_BUS_FMT_YVYU8_2X8,
227 MEDIA_BUS_FMT_YUYV8_2X8,
228 MEDIA_BUS_FMT_UYVY8_2X8,
229 MEDIA_BUS_FMT_VYUY8_2X8,
230 MEDIA_BUS_FMT_YVYU8_1_5X8,
231 };
232
233 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
234 index, src_req_code);
235 }
236 case MEDIA_BUS_FMT_UYVY8_2X8:
237 {
238 u32 src_code[] = {
239 MEDIA_BUS_FMT_UYVY8_2X8,
240 MEDIA_BUS_FMT_YUYV8_2X8,
241 MEDIA_BUS_FMT_YVYU8_2X8,
242 MEDIA_BUS_FMT_VYUY8_2X8,
243 MEDIA_BUS_FMT_UYVY8_1_5X8,
244 };
245
246 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
247 index, src_req_code);
248 }
249 case MEDIA_BUS_FMT_VYUY8_2X8:
250 {
251 u32 src_code[] = {
252 MEDIA_BUS_FMT_VYUY8_2X8,
253 MEDIA_BUS_FMT_YUYV8_2X8,
254 MEDIA_BUS_FMT_YVYU8_2X8,
255 MEDIA_BUS_FMT_UYVY8_2X8,
256 MEDIA_BUS_FMT_VYUY8_1_5X8,
257 };
258
259 return vfe_find_code(src_code, ARRAY_SIZE(src_code),
260 index, src_req_code);
261 }
262 default:
263 if (index > 0)
264 return 0;
265
266 return sink_code;
267 }
268 else
269 return 0;
270 }
271
272
273
274
275
276
277
278 static int vfe_reset(struct vfe_device *vfe)
279 {
280 unsigned long time;
281
282 reinit_completion(&vfe->reset_complete);
283
284 vfe->ops->global_reset(vfe);
285
286 time = wait_for_completion_timeout(&vfe->reset_complete,
287 msecs_to_jiffies(VFE_RESET_TIMEOUT_MS));
288 if (!time) {
289 dev_err(vfe->camss->dev, "VFE reset timeout\n");
290 return -EIO;
291 }
292
293 return 0;
294 }
295
296
297
298
299
300
301
302 static int vfe_halt(struct vfe_device *vfe)
303 {
304 unsigned long time;
305
306 reinit_completion(&vfe->halt_complete);
307
308 vfe->ops->halt_request(vfe);
309
310 time = wait_for_completion_timeout(&vfe->halt_complete,
311 msecs_to_jiffies(VFE_HALT_TIMEOUT_MS));
312 if (!time) {
313 dev_err(vfe->camss->dev, "VFE halt timeout\n");
314 return -EIO;
315 }
316
317 return 0;
318 }
319
320 static void vfe_init_outputs(struct vfe_device *vfe)
321 {
322 int i;
323
324 for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
325 struct vfe_output *output = &vfe->line[i].output;
326
327 output->state = VFE_OUTPUT_OFF;
328 output->buf[0] = NULL;
329 output->buf[1] = NULL;
330 INIT_LIST_HEAD(&output->pending_bufs);
331 }
332 }
333
334 static void vfe_reset_output_maps(struct vfe_device *vfe)
335 {
336 int i;
337
338 for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
339 vfe->wm_output_map[i] = VFE_LINE_NONE;
340 }
341
342 static void vfe_output_init_addrs(struct vfe_device *vfe,
343 struct vfe_output *output, u8 sync)
344 {
345 u32 ping_addr;
346 u32 pong_addr;
347 unsigned int i;
348
349 output->active_buf = 0;
350
351 for (i = 0; i < output->wm_num; i++) {
352 if (output->buf[0])
353 ping_addr = output->buf[0]->addr[i];
354 else
355 ping_addr = 0;
356
357 if (output->buf[1])
358 pong_addr = output->buf[1]->addr[i];
359 else
360 pong_addr = ping_addr;
361
362 vfe->ops->wm_set_ping_addr(vfe, output->wm_idx[i], ping_addr);
363 vfe->ops->wm_set_pong_addr(vfe, output->wm_idx[i], pong_addr);
364 if (sync)
365 vfe->ops->bus_reload_wm(vfe, output->wm_idx[i]);
366 }
367 }
368
369 static void vfe_output_update_ping_addr(struct vfe_device *vfe,
370 struct vfe_output *output, u8 sync)
371 {
372 u32 addr;
373 unsigned int i;
374
375 for (i = 0; i < output->wm_num; i++) {
376 if (output->buf[0])
377 addr = output->buf[0]->addr[i];
378 else
379 addr = 0;
380
381 vfe->ops->wm_set_ping_addr(vfe, output->wm_idx[i], addr);
382 if (sync)
383 vfe->ops->bus_reload_wm(vfe, output->wm_idx[i]);
384 }
385 }
386
387 static void vfe_output_update_pong_addr(struct vfe_device *vfe,
388 struct vfe_output *output, u8 sync)
389 {
390 u32 addr;
391 unsigned int i;
392
393 for (i = 0; i < output->wm_num; i++) {
394 if (output->buf[1])
395 addr = output->buf[1]->addr[i];
396 else
397 addr = 0;
398
399 vfe->ops->wm_set_pong_addr(vfe, output->wm_idx[i], addr);
400 if (sync)
401 vfe->ops->bus_reload_wm(vfe, output->wm_idx[i]);
402 }
403
404 }
405
406 static int vfe_reserve_wm(struct vfe_device *vfe, enum vfe_line_id line_id)
407 {
408 int ret = -EBUSY;
409 int i;
410
411 for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++) {
412 if (vfe->wm_output_map[i] == VFE_LINE_NONE) {
413 vfe->wm_output_map[i] = line_id;
414 ret = i;
415 break;
416 }
417 }
418
419 return ret;
420 }
421
422 static int vfe_release_wm(struct vfe_device *vfe, u8 wm)
423 {
424 if (wm >= ARRAY_SIZE(vfe->wm_output_map))
425 return -EINVAL;
426
427 vfe->wm_output_map[wm] = VFE_LINE_NONE;
428
429 return 0;
430 }
431
432 static void vfe_output_frame_drop(struct vfe_device *vfe,
433 struct vfe_output *output,
434 u32 drop_pattern)
435 {
436 u8 drop_period;
437 unsigned int i;
438
439
440 output->drop_update_idx++;
441 output->drop_update_idx %= VFE_FRAME_DROP_UPDATES;
442 drop_period = VFE_FRAME_DROP_VAL + output->drop_update_idx;
443
444 for (i = 0; i < output->wm_num; i++) {
445 vfe->ops->wm_set_framedrop_period(vfe, output->wm_idx[i],
446 drop_period);
447 vfe->ops->wm_set_framedrop_pattern(vfe, output->wm_idx[i],
448 drop_pattern);
449 }
450 vfe->ops->reg_update(vfe,
451 container_of(output, struct vfe_line, output)->id);
452 }
453
454 static struct camss_buffer *vfe_buf_get_pending(struct vfe_output *output)
455 {
456 struct camss_buffer *buffer = NULL;
457
458 if (!list_empty(&output->pending_bufs)) {
459 buffer = list_first_entry(&output->pending_bufs,
460 struct camss_buffer,
461 queue);
462 list_del(&buffer->queue);
463 }
464
465 return buffer;
466 }
467
468
469
470
471
472
473 static void vfe_buf_add_pending(struct vfe_output *output,
474 struct camss_buffer *buffer)
475 {
476 INIT_LIST_HEAD(&buffer->queue);
477 list_add_tail(&buffer->queue, &output->pending_bufs);
478 }
479
480
481
482
483
484
485 static void vfe_buf_flush_pending(struct vfe_output *output,
486 enum vb2_buffer_state state)
487 {
488 struct camss_buffer *buf;
489 struct camss_buffer *t;
490
491 list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
492 vb2_buffer_done(&buf->vb.vb2_buf, state);
493 list_del(&buf->queue);
494 }
495 }
496
497 static void vfe_buf_update_wm_on_next(struct vfe_device *vfe,
498 struct vfe_output *output)
499 {
500 switch (output->state) {
501 case VFE_OUTPUT_CONTINUOUS:
502 vfe_output_frame_drop(vfe, output, 3);
503 break;
504 case VFE_OUTPUT_SINGLE:
505 default:
506 dev_err_ratelimited(vfe->camss->dev,
507 "Next buf in wrong state! %d\n",
508 output->state);
509 break;
510 }
511 }
512
513 static void vfe_buf_update_wm_on_last(struct vfe_device *vfe,
514 struct vfe_output *output)
515 {
516 switch (output->state) {
517 case VFE_OUTPUT_CONTINUOUS:
518 output->state = VFE_OUTPUT_SINGLE;
519 vfe_output_frame_drop(vfe, output, 1);
520 break;
521 case VFE_OUTPUT_SINGLE:
522 output->state = VFE_OUTPUT_STOPPING;
523 vfe_output_frame_drop(vfe, output, 0);
524 break;
525 default:
526 dev_err_ratelimited(vfe->camss->dev,
527 "Last buff in wrong state! %d\n",
528 output->state);
529 break;
530 }
531 }
532
533 static void vfe_buf_update_wm_on_new(struct vfe_device *vfe,
534 struct vfe_output *output,
535 struct camss_buffer *new_buf)
536 {
537 int inactive_idx;
538
539 switch (output->state) {
540 case VFE_OUTPUT_SINGLE:
541 inactive_idx = !output->active_buf;
542
543 if (!output->buf[inactive_idx]) {
544 output->buf[inactive_idx] = new_buf;
545
546 if (inactive_idx)
547 vfe_output_update_pong_addr(vfe, output, 0);
548 else
549 vfe_output_update_ping_addr(vfe, output, 0);
550
551 vfe_output_frame_drop(vfe, output, 3);
552 output->state = VFE_OUTPUT_CONTINUOUS;
553 } else {
554 vfe_buf_add_pending(output, new_buf);
555 dev_err_ratelimited(vfe->camss->dev,
556 "Inactive buffer is busy\n");
557 }
558 break;
559
560 case VFE_OUTPUT_IDLE:
561 if (!output->buf[0]) {
562 output->buf[0] = new_buf;
563
564 vfe_output_init_addrs(vfe, output, 1);
565
566 vfe_output_frame_drop(vfe, output, 1);
567 output->state = VFE_OUTPUT_SINGLE;
568 } else {
569 vfe_buf_add_pending(output, new_buf);
570 dev_err_ratelimited(vfe->camss->dev,
571 "Output idle with buffer set!\n");
572 }
573 break;
574
575 case VFE_OUTPUT_CONTINUOUS:
576 default:
577 vfe_buf_add_pending(output, new_buf);
578 break;
579 }
580 }
581
582 static int vfe_get_output(struct vfe_line *line)
583 {
584 struct vfe_device *vfe = to_vfe(line);
585 struct vfe_output *output;
586 struct v4l2_format *f = &line->video_out.active_fmt;
587 unsigned long flags;
588 int i;
589 int wm_idx;
590
591 spin_lock_irqsave(&vfe->output_lock, flags);
592
593 output = &line->output;
594 if (output->state != VFE_OUTPUT_OFF) {
595 dev_err(vfe->camss->dev, "Output is running\n");
596 goto error;
597 }
598 output->state = VFE_OUTPUT_RESERVED;
599
600 output->active_buf = 0;
601
602 switch (f->fmt.pix_mp.pixelformat) {
603 case V4L2_PIX_FMT_NV12:
604 case V4L2_PIX_FMT_NV21:
605 case V4L2_PIX_FMT_NV16:
606 case V4L2_PIX_FMT_NV61:
607 output->wm_num = 2;
608 break;
609 default:
610 output->wm_num = 1;
611 break;
612 }
613
614 for (i = 0; i < output->wm_num; i++) {
615 wm_idx = vfe_reserve_wm(vfe, line->id);
616 if (wm_idx < 0) {
617 dev_err(vfe->camss->dev, "Can not reserve wm\n");
618 goto error_get_wm;
619 }
620 output->wm_idx[i] = wm_idx;
621 }
622
623 output->drop_update_idx = 0;
624
625 spin_unlock_irqrestore(&vfe->output_lock, flags);
626
627 return 0;
628
629 error_get_wm:
630 for (i--; i >= 0; i--)
631 vfe_release_wm(vfe, output->wm_idx[i]);
632 output->state = VFE_OUTPUT_OFF;
633 error:
634 spin_unlock_irqrestore(&vfe->output_lock, flags);
635
636 return -EINVAL;
637 }
638
639 static int vfe_put_output(struct vfe_line *line)
640 {
641 struct vfe_device *vfe = to_vfe(line);
642 struct vfe_output *output = &line->output;
643 unsigned long flags;
644 unsigned int i;
645
646 spin_lock_irqsave(&vfe->output_lock, flags);
647
648 for (i = 0; i < output->wm_num; i++)
649 vfe_release_wm(vfe, output->wm_idx[i]);
650
651 output->state = VFE_OUTPUT_OFF;
652
653 spin_unlock_irqrestore(&vfe->output_lock, flags);
654 return 0;
655 }
656
657 static int vfe_enable_output(struct vfe_line *line)
658 {
659 struct vfe_device *vfe = to_vfe(line);
660 struct vfe_output *output = &line->output;
661 const struct vfe_hw_ops *ops = vfe->ops;
662 struct media_entity *sensor;
663 unsigned long flags;
664 unsigned int frame_skip = 0;
665 unsigned int i;
666 u16 ub_size;
667
668 ub_size = ops->get_ub_size(vfe->id);
669 if (!ub_size)
670 return -EINVAL;
671
672 sensor = camss_find_sensor(&line->subdev.entity);
673 if (sensor) {
674 struct v4l2_subdev *subdev =
675 media_entity_to_v4l2_subdev(sensor);
676
677 v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
678
679 if (frame_skip > VFE_FRAME_DROP_VAL - 1)
680 frame_skip = VFE_FRAME_DROP_VAL - 1;
681 }
682
683 spin_lock_irqsave(&vfe->output_lock, flags);
684
685 ops->reg_update_clear(vfe, line->id);
686
687 if (output->state != VFE_OUTPUT_RESERVED) {
688 dev_err(vfe->camss->dev, "Output is not in reserved state %d\n",
689 output->state);
690 spin_unlock_irqrestore(&vfe->output_lock, flags);
691 return -EINVAL;
692 }
693 output->state = VFE_OUTPUT_IDLE;
694
695 output->buf[0] = vfe_buf_get_pending(output);
696 output->buf[1] = vfe_buf_get_pending(output);
697
698 if (!output->buf[0] && output->buf[1]) {
699 output->buf[0] = output->buf[1];
700 output->buf[1] = NULL;
701 }
702
703 if (output->buf[0])
704 output->state = VFE_OUTPUT_SINGLE;
705
706 if (output->buf[1])
707 output->state = VFE_OUTPUT_CONTINUOUS;
708
709 switch (output->state) {
710 case VFE_OUTPUT_SINGLE:
711 vfe_output_frame_drop(vfe, output, 1 << frame_skip);
712 break;
713 case VFE_OUTPUT_CONTINUOUS:
714 vfe_output_frame_drop(vfe, output, 3 << frame_skip);
715 break;
716 default:
717 vfe_output_frame_drop(vfe, output, 0);
718 break;
719 }
720
721 output->sequence = 0;
722 output->wait_sof = 0;
723 output->wait_reg_update = 0;
724 reinit_completion(&output->sof);
725 reinit_completion(&output->reg_update);
726
727 vfe_output_init_addrs(vfe, output, 0);
728
729 if (line->id != VFE_LINE_PIX) {
730 ops->set_cgc_override(vfe, output->wm_idx[0], 1);
731 ops->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 1);
732 ops->bus_connect_wm_to_rdi(vfe, output->wm_idx[0], line->id);
733 ops->wm_set_subsample(vfe, output->wm_idx[0]);
734 ops->set_rdi_cid(vfe, line->id, 0);
735 ops->wm_set_ub_cfg(vfe, output->wm_idx[0],
736 (ub_size + 1) * output->wm_idx[0], ub_size);
737 ops->wm_frame_based(vfe, output->wm_idx[0], 1);
738 ops->wm_enable(vfe, output->wm_idx[0], 1);
739 ops->bus_reload_wm(vfe, output->wm_idx[0]);
740 } else {
741 ub_size /= output->wm_num;
742 for (i = 0; i < output->wm_num; i++) {
743 ops->set_cgc_override(vfe, output->wm_idx[i], 1);
744 ops->wm_set_subsample(vfe, output->wm_idx[i]);
745 ops->wm_set_ub_cfg(vfe, output->wm_idx[i],
746 (ub_size + 1) * output->wm_idx[i],
747 ub_size);
748 ops->wm_line_based(vfe, output->wm_idx[i],
749 &line->video_out.active_fmt.fmt.pix_mp,
750 i, 1);
751 ops->wm_enable(vfe, output->wm_idx[i], 1);
752 ops->bus_reload_wm(vfe, output->wm_idx[i]);
753 }
754 ops->enable_irq_pix_line(vfe, 0, line->id, 1);
755 ops->set_module_cfg(vfe, 1);
756 ops->set_camif_cfg(vfe, line);
757 ops->set_realign_cfg(vfe, line, 1);
758 ops->set_xbar_cfg(vfe, output, 1);
759 ops->set_demux_cfg(vfe, line);
760 ops->set_scale_cfg(vfe, line);
761 ops->set_crop_cfg(vfe, line);
762 ops->set_clamp_cfg(vfe);
763 ops->set_camif_cmd(vfe, 1);
764 }
765
766 ops->reg_update(vfe, line->id);
767
768 spin_unlock_irqrestore(&vfe->output_lock, flags);
769
770 return 0;
771 }
772
773 static int vfe_disable_output(struct vfe_line *line)
774 {
775 struct vfe_device *vfe = to_vfe(line);
776 struct vfe_output *output = &line->output;
777 const struct vfe_hw_ops *ops = vfe->ops;
778 unsigned long flags;
779 unsigned long time;
780 unsigned int i;
781
782 spin_lock_irqsave(&vfe->output_lock, flags);
783
784 output->wait_sof = 1;
785 spin_unlock_irqrestore(&vfe->output_lock, flags);
786
787 time = wait_for_completion_timeout(&output->sof,
788 msecs_to_jiffies(VFE_NEXT_SOF_MS));
789 if (!time)
790 dev_err(vfe->camss->dev, "VFE sof timeout\n");
791
792 spin_lock_irqsave(&vfe->output_lock, flags);
793 for (i = 0; i < output->wm_num; i++)
794 ops->wm_enable(vfe, output->wm_idx[i], 0);
795
796 ops->reg_update(vfe, line->id);
797 output->wait_reg_update = 1;
798 spin_unlock_irqrestore(&vfe->output_lock, flags);
799
800 time = wait_for_completion_timeout(&output->reg_update,
801 msecs_to_jiffies(VFE_NEXT_SOF_MS));
802 if (!time)
803 dev_err(vfe->camss->dev, "VFE reg update timeout\n");
804
805 spin_lock_irqsave(&vfe->output_lock, flags);
806
807 if (line->id != VFE_LINE_PIX) {
808 ops->wm_frame_based(vfe, output->wm_idx[0], 0);
809 ops->bus_disconnect_wm_from_rdi(vfe, output->wm_idx[0],
810 line->id);
811 ops->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 0);
812 ops->set_cgc_override(vfe, output->wm_idx[0], 0);
813 spin_unlock_irqrestore(&vfe->output_lock, flags);
814 } else {
815 for (i = 0; i < output->wm_num; i++) {
816 ops->wm_line_based(vfe, output->wm_idx[i], NULL, i, 0);
817 ops->set_cgc_override(vfe, output->wm_idx[i], 0);
818 }
819
820 ops->enable_irq_pix_line(vfe, 0, line->id, 0);
821 ops->set_module_cfg(vfe, 0);
822 ops->set_realign_cfg(vfe, line, 0);
823 ops->set_xbar_cfg(vfe, output, 0);
824
825 ops->set_camif_cmd(vfe, 0);
826 spin_unlock_irqrestore(&vfe->output_lock, flags);
827
828 ops->camif_wait_for_stop(vfe, vfe->camss->dev);
829 }
830
831 return 0;
832 }
833
834
835
836
837
838
839
840 static int vfe_enable(struct vfe_line *line)
841 {
842 struct vfe_device *vfe = to_vfe(line);
843 int ret;
844
845 mutex_lock(&vfe->stream_lock);
846
847 if (!vfe->stream_count) {
848 vfe->ops->enable_irq_common(vfe);
849
850 vfe->ops->bus_enable_wr_if(vfe, 1);
851
852 vfe->ops->set_qos(vfe);
853
854 vfe->ops->set_ds(vfe);
855 }
856
857 vfe->stream_count++;
858
859 mutex_unlock(&vfe->stream_lock);
860
861 ret = vfe_get_output(line);
862 if (ret < 0)
863 goto error_get_output;
864
865 ret = vfe_enable_output(line);
866 if (ret < 0)
867 goto error_enable_output;
868
869 vfe->was_streaming = 1;
870
871 return 0;
872
873
874 error_enable_output:
875 vfe_put_output(line);
876
877 error_get_output:
878 mutex_lock(&vfe->stream_lock);
879
880 if (vfe->stream_count == 1)
881 vfe->ops->bus_enable_wr_if(vfe, 0);
882
883 vfe->stream_count--;
884
885 mutex_unlock(&vfe->stream_lock);
886
887 return ret;
888 }
889
890
891
892
893
894
895
896 static int vfe_disable(struct vfe_line *line)
897 {
898 struct vfe_device *vfe = to_vfe(line);
899
900 vfe_disable_output(line);
901
902 vfe_put_output(line);
903
904 mutex_lock(&vfe->stream_lock);
905
906 if (vfe->stream_count == 1)
907 vfe->ops->bus_enable_wr_if(vfe, 0);
908
909 vfe->stream_count--;
910
911 mutex_unlock(&vfe->stream_lock);
912
913 return 0;
914 }
915
916
917
918
919
920
921 static void vfe_isr_sof(struct vfe_device *vfe, enum vfe_line_id line_id)
922 {
923 struct vfe_output *output;
924 unsigned long flags;
925
926 spin_lock_irqsave(&vfe->output_lock, flags);
927 output = &vfe->line[line_id].output;
928 if (output->wait_sof) {
929 output->wait_sof = 0;
930 complete(&output->sof);
931 }
932 spin_unlock_irqrestore(&vfe->output_lock, flags);
933 }
934
935
936
937
938
939
940 static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
941 {
942 struct vfe_output *output;
943 unsigned long flags;
944
945 spin_lock_irqsave(&vfe->output_lock, flags);
946 vfe->ops->reg_update_clear(vfe, line_id);
947
948 output = &vfe->line[line_id].output;
949
950 if (output->wait_reg_update) {
951 output->wait_reg_update = 0;
952 complete(&output->reg_update);
953 spin_unlock_irqrestore(&vfe->output_lock, flags);
954 return;
955 }
956
957 if (output->state == VFE_OUTPUT_STOPPING) {
958
959 if (output->last_buffer) {
960 vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
961 VB2_BUF_STATE_DONE);
962 output->last_buffer = NULL;
963 }
964 output->state = VFE_OUTPUT_IDLE;
965
966
967
968
969 output->buf[0] = vfe_buf_get_pending(output);
970 output->buf[1] = vfe_buf_get_pending(output);
971
972 if (!output->buf[0] && output->buf[1]) {
973 output->buf[0] = output->buf[1];
974 output->buf[1] = NULL;
975 }
976
977 if (output->buf[0])
978 output->state = VFE_OUTPUT_SINGLE;
979
980 if (output->buf[1])
981 output->state = VFE_OUTPUT_CONTINUOUS;
982
983 switch (output->state) {
984 case VFE_OUTPUT_SINGLE:
985 vfe_output_frame_drop(vfe, output, 2);
986 break;
987 case VFE_OUTPUT_CONTINUOUS:
988 vfe_output_frame_drop(vfe, output, 3);
989 break;
990 default:
991 vfe_output_frame_drop(vfe, output, 0);
992 break;
993 }
994
995 vfe_output_init_addrs(vfe, output, 1);
996 }
997
998 spin_unlock_irqrestore(&vfe->output_lock, flags);
999 }
1000
1001
1002
1003
1004
1005
1006 static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
1007 {
1008 struct camss_buffer *ready_buf;
1009 struct vfe_output *output;
1010 dma_addr_t *new_addr;
1011 unsigned long flags;
1012 u32 active_index;
1013 u64 ts = ktime_get_ns();
1014 unsigned int i;
1015
1016 active_index = vfe->ops->wm_get_ping_pong_status(vfe, wm);
1017
1018 spin_lock_irqsave(&vfe->output_lock, flags);
1019
1020 if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
1021 dev_err_ratelimited(vfe->camss->dev,
1022 "Received wm done for unmapped index\n");
1023 goto out_unlock;
1024 }
1025 output = &vfe->line[vfe->wm_output_map[wm]].output;
1026
1027 if (output->active_buf == active_index) {
1028 dev_err_ratelimited(vfe->camss->dev,
1029 "Active buffer mismatch!\n");
1030 goto out_unlock;
1031 }
1032 output->active_buf = active_index;
1033
1034 ready_buf = output->buf[!active_index];
1035 if (!ready_buf) {
1036 dev_err_ratelimited(vfe->camss->dev,
1037 "Missing ready buf %d %d!\n",
1038 !active_index, output->state);
1039 goto out_unlock;
1040 }
1041
1042 ready_buf->vb.vb2_buf.timestamp = ts;
1043 ready_buf->vb.sequence = output->sequence++;
1044
1045
1046 output->buf[!active_index] = vfe_buf_get_pending(output);
1047 if (!output->buf[!active_index]) {
1048
1049 new_addr = ready_buf->addr;
1050 vfe_buf_update_wm_on_last(vfe, output);
1051 } else {
1052 new_addr = output->buf[!active_index]->addr;
1053 vfe_buf_update_wm_on_next(vfe, output);
1054 }
1055
1056 if (active_index)
1057 for (i = 0; i < output->wm_num; i++)
1058 vfe->ops->wm_set_ping_addr(vfe, output->wm_idx[i],
1059 new_addr[i]);
1060 else
1061 for (i = 0; i < output->wm_num; i++)
1062 vfe->ops->wm_set_pong_addr(vfe, output->wm_idx[i],
1063 new_addr[i]);
1064
1065 spin_unlock_irqrestore(&vfe->output_lock, flags);
1066
1067 if (output->state == VFE_OUTPUT_STOPPING)
1068 output->last_buffer = ready_buf;
1069 else
1070 vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
1071
1072 return;
1073
1074 out_unlock:
1075 spin_unlock_irqrestore(&vfe->output_lock, flags);
1076 }
1077
1078
1079
1080
1081
1082
1083 static void vfe_isr_comp_done(struct vfe_device *vfe, u8 comp)
1084 {
1085 unsigned int i;
1086
1087 for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
1088 if (vfe->wm_output_map[i] == VFE_LINE_PIX) {
1089 vfe_isr_wm_done(vfe, i);
1090 break;
1091 }
1092 }
1093
1094 static inline void vfe_isr_reset_ack(struct vfe_device *vfe)
1095 {
1096 complete(&vfe->reset_complete);
1097 }
1098
1099 static inline void vfe_isr_halt_ack(struct vfe_device *vfe)
1100 {
1101 complete(&vfe->halt_complete);
1102 vfe->ops->halt_clear(vfe);
1103 }
1104
1105
1106
1107
1108
1109
1110
1111 static int vfe_set_clock_rates(struct vfe_device *vfe)
1112 {
1113 struct device *dev = vfe->camss->dev;
1114 u32 pixel_clock[MSM_VFE_LINE_NUM];
1115 int i, j;
1116 int ret;
1117
1118 for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
1119 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
1120 &pixel_clock[i]);
1121 if (ret)
1122 pixel_clock[i] = 0;
1123 }
1124
1125 for (i = 0; i < vfe->nclocks; i++) {
1126 struct camss_clock *clock = &vfe->clock[i];
1127
1128 if (!strcmp(clock->name, "vfe0") ||
1129 !strcmp(clock->name, "vfe1")) {
1130 u64 min_rate = 0;
1131 long rate;
1132
1133 for (j = VFE_LINE_RDI0; j <= VFE_LINE_PIX; j++) {
1134 u32 tmp;
1135 u8 bpp;
1136
1137 if (j == VFE_LINE_PIX) {
1138 tmp = pixel_clock[j];
1139 } else {
1140 struct vfe_line *l = &vfe->line[j];
1141
1142 bpp = vfe_get_bpp(l->formats,
1143 l->nformats,
1144 l->fmt[MSM_VFE_PAD_SINK].code);
1145 tmp = pixel_clock[j] * bpp / 64;
1146 }
1147
1148 if (min_rate < tmp)
1149 min_rate = tmp;
1150 }
1151
1152 camss_add_clock_margin(&min_rate);
1153
1154 for (j = 0; j < clock->nfreqs; j++)
1155 if (min_rate < clock->freq[j])
1156 break;
1157
1158 if (j == clock->nfreqs) {
1159 dev_err(dev,
1160 "Pixel clock is too high for VFE");
1161 return -EINVAL;
1162 }
1163
1164
1165
1166 if (min_rate == 0)
1167 j = clock->nfreqs - 1;
1168
1169 rate = clk_round_rate(clock->clk, clock->freq[j]);
1170 if (rate < 0) {
1171 dev_err(dev, "clk round rate failed: %ld\n",
1172 rate);
1173 return -EINVAL;
1174 }
1175
1176 ret = clk_set_rate(clock->clk, rate);
1177 if (ret < 0) {
1178 dev_err(dev, "clk set rate failed: %d\n", ret);
1179 return ret;
1180 }
1181 }
1182 }
1183
1184 return 0;
1185 }
1186
1187
1188
1189
1190
1191
1192
1193
1194 static int vfe_check_clock_rates(struct vfe_device *vfe)
1195 {
1196 u32 pixel_clock[MSM_VFE_LINE_NUM];
1197 int i, j;
1198 int ret;
1199
1200 for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
1201 ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
1202 &pixel_clock[i]);
1203 if (ret)
1204 pixel_clock[i] = 0;
1205 }
1206
1207 for (i = 0; i < vfe->nclocks; i++) {
1208 struct camss_clock *clock = &vfe->clock[i];
1209
1210 if (!strcmp(clock->name, "vfe0") ||
1211 !strcmp(clock->name, "vfe1")) {
1212 u64 min_rate = 0;
1213 unsigned long rate;
1214
1215 for (j = VFE_LINE_RDI0; j <= VFE_LINE_PIX; j++) {
1216 u32 tmp;
1217 u8 bpp;
1218
1219 if (j == VFE_LINE_PIX) {
1220 tmp = pixel_clock[j];
1221 } else {
1222 struct vfe_line *l = &vfe->line[j];
1223
1224 bpp = vfe_get_bpp(l->formats,
1225 l->nformats,
1226 l->fmt[MSM_VFE_PAD_SINK].code);
1227 tmp = pixel_clock[j] * bpp / 64;
1228 }
1229
1230 if (min_rate < tmp)
1231 min_rate = tmp;
1232 }
1233
1234 camss_add_clock_margin(&min_rate);
1235
1236 rate = clk_get_rate(clock->clk);
1237 if (rate < min_rate)
1238 return -EBUSY;
1239 }
1240 }
1241
1242 return 0;
1243 }
1244
1245
1246
1247
1248
1249
1250
1251 static int vfe_get(struct vfe_device *vfe)
1252 {
1253 int ret;
1254
1255 mutex_lock(&vfe->power_lock);
1256
1257 if (vfe->power_count == 0) {
1258 ret = camss_pm_domain_on(vfe->camss, vfe->id);
1259 if (ret < 0)
1260 goto error_pm_domain;
1261
1262 ret = pm_runtime_get_sync(vfe->camss->dev);
1263 if (ret < 0)
1264 goto error_pm_runtime_get;
1265
1266 ret = vfe_set_clock_rates(vfe);
1267 if (ret < 0)
1268 goto error_clocks;
1269
1270 ret = camss_enable_clocks(vfe->nclocks, vfe->clock,
1271 vfe->camss->dev);
1272 if (ret < 0)
1273 goto error_clocks;
1274
1275 ret = vfe_reset(vfe);
1276 if (ret < 0)
1277 goto error_reset;
1278
1279 vfe_reset_output_maps(vfe);
1280
1281 vfe_init_outputs(vfe);
1282 } else {
1283 ret = vfe_check_clock_rates(vfe);
1284 if (ret < 0)
1285 goto error_clocks;
1286 }
1287 vfe->power_count++;
1288
1289 mutex_unlock(&vfe->power_lock);
1290
1291 return 0;
1292
1293 error_reset:
1294 camss_disable_clocks(vfe->nclocks, vfe->clock);
1295
1296 error_clocks:
1297 pm_runtime_put_sync(vfe->camss->dev);
1298
1299 error_pm_runtime_get:
1300 camss_pm_domain_off(vfe->camss, vfe->id);
1301
1302 error_pm_domain:
1303 mutex_unlock(&vfe->power_lock);
1304
1305 return ret;
1306 }
1307
1308
1309
1310
1311
1312 static void vfe_put(struct vfe_device *vfe)
1313 {
1314 mutex_lock(&vfe->power_lock);
1315
1316 if (vfe->power_count == 0) {
1317 dev_err(vfe->camss->dev, "vfe power off on power_count == 0\n");
1318 goto exit;
1319 } else if (vfe->power_count == 1) {
1320 if (vfe->was_streaming) {
1321 vfe->was_streaming = 0;
1322 vfe_halt(vfe);
1323 }
1324 camss_disable_clocks(vfe->nclocks, vfe->clock);
1325 pm_runtime_put_sync(vfe->camss->dev);
1326 camss_pm_domain_off(vfe->camss, vfe->id);
1327 }
1328
1329 vfe->power_count--;
1330
1331 exit:
1332 mutex_unlock(&vfe->power_lock);
1333 }
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345 static int vfe_queue_buffer(struct camss_video *vid,
1346 struct camss_buffer *buf)
1347 {
1348 struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
1349 struct vfe_device *vfe = to_vfe(line);
1350 struct vfe_output *output;
1351 unsigned long flags;
1352
1353 output = &line->output;
1354
1355 spin_lock_irqsave(&vfe->output_lock, flags);
1356
1357 vfe_buf_update_wm_on_new(vfe, output, buf);
1358
1359 spin_unlock_irqrestore(&vfe->output_lock, flags);
1360
1361 return 0;
1362 }
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374 static int vfe_flush_buffers(struct camss_video *vid,
1375 enum vb2_buffer_state state)
1376 {
1377 struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
1378 struct vfe_device *vfe = to_vfe(line);
1379 struct vfe_output *output;
1380 unsigned long flags;
1381
1382 output = &line->output;
1383
1384 spin_lock_irqsave(&vfe->output_lock, flags);
1385
1386 vfe_buf_flush_pending(output, state);
1387
1388 if (output->buf[0])
1389 vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
1390
1391 if (output->buf[1])
1392 vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
1393
1394 if (output->last_buffer) {
1395 vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
1396 output->last_buffer = NULL;
1397 }
1398
1399 spin_unlock_irqrestore(&vfe->output_lock, flags);
1400
1401 return 0;
1402 }
1403
1404
1405
1406
1407
1408
1409
1410
1411 static int vfe_set_power(struct v4l2_subdev *sd, int on)
1412 {
1413 struct vfe_line *line = v4l2_get_subdevdata(sd);
1414 struct vfe_device *vfe = to_vfe(line);
1415 int ret;
1416
1417 if (on) {
1418 ret = vfe_get(vfe);
1419 if (ret < 0)
1420 return ret;
1421
1422 vfe->ops->hw_version_read(vfe, vfe->camss->dev);
1423 } else {
1424 vfe_put(vfe);
1425 }
1426
1427 return 0;
1428 }
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439 static int vfe_set_stream(struct v4l2_subdev *sd, int enable)
1440 {
1441 struct vfe_line *line = v4l2_get_subdevdata(sd);
1442 struct vfe_device *vfe = to_vfe(line);
1443 int ret;
1444
1445 if (enable) {
1446 ret = vfe_enable(line);
1447 if (ret < 0)
1448 dev_err(vfe->camss->dev,
1449 "Failed to enable vfe outputs\n");
1450 } else {
1451 ret = vfe_disable(line);
1452 if (ret < 0)
1453 dev_err(vfe->camss->dev,
1454 "Failed to disable vfe outputs\n");
1455 }
1456
1457 return ret;
1458 }
1459
1460
1461
1462
1463
1464
1465
1466
1467
1468
1469 static struct v4l2_mbus_framefmt *
1470 __vfe_get_format(struct vfe_line *line,
1471 struct v4l2_subdev_pad_config *cfg,
1472 unsigned int pad,
1473 enum v4l2_subdev_format_whence which)
1474 {
1475 if (which == V4L2_SUBDEV_FORMAT_TRY)
1476 return v4l2_subdev_get_try_format(&line->subdev, cfg, pad);
1477
1478 return &line->fmt[pad];
1479 }
1480
1481
1482
1483
1484
1485
1486
1487
1488
1489 static struct v4l2_rect *
1490 __vfe_get_compose(struct vfe_line *line,
1491 struct v4l2_subdev_pad_config *cfg,
1492 enum v4l2_subdev_format_whence which)
1493 {
1494 if (which == V4L2_SUBDEV_FORMAT_TRY)
1495 return v4l2_subdev_get_try_compose(&line->subdev, cfg,
1496 MSM_VFE_PAD_SINK);
1497
1498 return &line->compose;
1499 }
1500
1501
1502
1503
1504
1505
1506
1507
1508
1509 static struct v4l2_rect *
1510 __vfe_get_crop(struct vfe_line *line,
1511 struct v4l2_subdev_pad_config *cfg,
1512 enum v4l2_subdev_format_whence which)
1513 {
1514 if (which == V4L2_SUBDEV_FORMAT_TRY)
1515 return v4l2_subdev_get_try_crop(&line->subdev, cfg,
1516 MSM_VFE_PAD_SRC);
1517
1518 return &line->crop;
1519 }
1520
1521
1522
1523
1524
1525
1526
1527
1528
1529 static void vfe_try_format(struct vfe_line *line,
1530 struct v4l2_subdev_pad_config *cfg,
1531 unsigned int pad,
1532 struct v4l2_mbus_framefmt *fmt,
1533 enum v4l2_subdev_format_whence which)
1534 {
1535 unsigned int i;
1536 u32 code;
1537
1538 switch (pad) {
1539 case MSM_VFE_PAD_SINK:
1540
1541
1542 for (i = 0; i < line->nformats; i++)
1543 if (fmt->code == line->formats[i].code)
1544 break;
1545
1546
1547 if (i >= line->nformats)
1548 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
1549
1550 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
1551 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
1552
1553 fmt->field = V4L2_FIELD_NONE;
1554 fmt->colorspace = V4L2_COLORSPACE_SRGB;
1555
1556 break;
1557
1558 case MSM_VFE_PAD_SRC:
1559
1560 code = fmt->code;
1561
1562 *fmt = *__vfe_get_format(line, cfg, MSM_VFE_PAD_SINK, which);
1563
1564 fmt->code = vfe_src_pad_code(line, fmt->code, 0, code);
1565
1566 if (line->id == VFE_LINE_PIX) {
1567 struct v4l2_rect *rect;
1568
1569 rect = __vfe_get_crop(line, cfg, which);
1570
1571 fmt->width = rect->width;
1572 fmt->height = rect->height;
1573 }
1574
1575 break;
1576 }
1577
1578 fmt->colorspace = V4L2_COLORSPACE_SRGB;
1579 }
1580
1581
1582
1583
1584
1585
1586
1587
1588 static void vfe_try_compose(struct vfe_line *line,
1589 struct v4l2_subdev_pad_config *cfg,
1590 struct v4l2_rect *rect,
1591 enum v4l2_subdev_format_whence which)
1592 {
1593 struct v4l2_mbus_framefmt *fmt;
1594
1595 fmt = __vfe_get_format(line, cfg, MSM_VFE_PAD_SINK, which);
1596
1597 if (rect->width > fmt->width)
1598 rect->width = fmt->width;
1599
1600 if (rect->height > fmt->height)
1601 rect->height = fmt->height;
1602
1603 if (fmt->width > rect->width * SCALER_RATIO_MAX)
1604 rect->width = (fmt->width + SCALER_RATIO_MAX - 1) /
1605 SCALER_RATIO_MAX;
1606
1607 rect->width &= ~0x1;
1608
1609 if (fmt->height > rect->height * SCALER_RATIO_MAX)
1610 rect->height = (fmt->height + SCALER_RATIO_MAX - 1) /
1611 SCALER_RATIO_MAX;
1612
1613 if (rect->width < 16)
1614 rect->width = 16;
1615
1616 if (rect->height < 4)
1617 rect->height = 4;
1618 }
1619
1620
1621
1622
1623
1624
1625
1626
1627 static void vfe_try_crop(struct vfe_line *line,
1628 struct v4l2_subdev_pad_config *cfg,
1629 struct v4l2_rect *rect,
1630 enum v4l2_subdev_format_whence which)
1631 {
1632 struct v4l2_rect *compose;
1633
1634 compose = __vfe_get_compose(line, cfg, which);
1635
1636 if (rect->width > compose->width)
1637 rect->width = compose->width;
1638
1639 if (rect->width + rect->left > compose->width)
1640 rect->left = compose->width - rect->width;
1641
1642 if (rect->height > compose->height)
1643 rect->height = compose->height;
1644
1645 if (rect->height + rect->top > compose->height)
1646 rect->top = compose->height - rect->height;
1647
1648
1649 rect->left += (rect->width & 0xf) >> 1;
1650 rect->width &= ~0xf;
1651
1652 if (rect->width < 16) {
1653 rect->left = 0;
1654 rect->width = 16;
1655 }
1656
1657 if (rect->height < 4) {
1658 rect->top = 0;
1659 rect->height = 4;
1660 }
1661 }
1662
1663
1664
1665
1666
1667
1668
1669
1670
1671 static int vfe_enum_mbus_code(struct v4l2_subdev *sd,
1672 struct v4l2_subdev_pad_config *cfg,
1673 struct v4l2_subdev_mbus_code_enum *code)
1674 {
1675 struct vfe_line *line = v4l2_get_subdevdata(sd);
1676
1677 if (code->pad == MSM_VFE_PAD_SINK) {
1678 if (code->index >= line->nformats)
1679 return -EINVAL;
1680
1681 code->code = line->formats[code->index].code;
1682 } else {
1683 struct v4l2_mbus_framefmt *sink_fmt;
1684
1685 sink_fmt = __vfe_get_format(line, cfg, MSM_VFE_PAD_SINK,
1686 code->which);
1687
1688 code->code = vfe_src_pad_code(line, sink_fmt->code,
1689 code->index, 0);
1690 if (!code->code)
1691 return -EINVAL;
1692 }
1693
1694 return 0;
1695 }
1696
1697
1698
1699
1700
1701
1702
1703
1704
1705 static int vfe_enum_frame_size(struct v4l2_subdev *sd,
1706 struct v4l2_subdev_pad_config *cfg,
1707 struct v4l2_subdev_frame_size_enum *fse)
1708 {
1709 struct vfe_line *line = v4l2_get_subdevdata(sd);
1710 struct v4l2_mbus_framefmt format;
1711
1712 if (fse->index != 0)
1713 return -EINVAL;
1714
1715 format.code = fse->code;
1716 format.width = 1;
1717 format.height = 1;
1718 vfe_try_format(line, cfg, fse->pad, &format, fse->which);
1719 fse->min_width = format.width;
1720 fse->min_height = format.height;
1721
1722 if (format.code != fse->code)
1723 return -EINVAL;
1724
1725 format.code = fse->code;
1726 format.width = -1;
1727 format.height = -1;
1728 vfe_try_format(line, cfg, fse->pad, &format, fse->which);
1729 fse->max_width = format.width;
1730 fse->max_height = format.height;
1731
1732 return 0;
1733 }
1734
1735
1736
1737
1738
1739
1740
1741
1742
1743 static int vfe_get_format(struct v4l2_subdev *sd,
1744 struct v4l2_subdev_pad_config *cfg,
1745 struct v4l2_subdev_format *fmt)
1746 {
1747 struct vfe_line *line = v4l2_get_subdevdata(sd);
1748 struct v4l2_mbus_framefmt *format;
1749
1750 format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
1751 if (format == NULL)
1752 return -EINVAL;
1753
1754 fmt->format = *format;
1755
1756 return 0;
1757 }
1758
1759 static int vfe_set_selection(struct v4l2_subdev *sd,
1760 struct v4l2_subdev_pad_config *cfg,
1761 struct v4l2_subdev_selection *sel);
1762
1763
1764
1765
1766
1767
1768
1769
1770
1771 static int vfe_set_format(struct v4l2_subdev *sd,
1772 struct v4l2_subdev_pad_config *cfg,
1773 struct v4l2_subdev_format *fmt)
1774 {
1775 struct vfe_line *line = v4l2_get_subdevdata(sd);
1776 struct v4l2_mbus_framefmt *format;
1777
1778 format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
1779 if (format == NULL)
1780 return -EINVAL;
1781
1782 vfe_try_format(line, cfg, fmt->pad, &fmt->format, fmt->which);
1783 *format = fmt->format;
1784
1785 if (fmt->pad == MSM_VFE_PAD_SINK) {
1786 struct v4l2_subdev_selection sel = { 0 };
1787 int ret;
1788
1789
1790 format = __vfe_get_format(line, cfg, MSM_VFE_PAD_SRC,
1791 fmt->which);
1792
1793 *format = fmt->format;
1794 vfe_try_format(line, cfg, MSM_VFE_PAD_SRC, format,
1795 fmt->which);
1796
1797 if (line->id != VFE_LINE_PIX)
1798 return 0;
1799
1800
1801 sel.which = fmt->which;
1802 sel.pad = MSM_VFE_PAD_SINK;
1803 sel.target = V4L2_SEL_TGT_COMPOSE;
1804 sel.r.width = fmt->format.width;
1805 sel.r.height = fmt->format.height;
1806 ret = vfe_set_selection(sd, cfg, &sel);
1807 if (ret < 0)
1808 return ret;
1809 }
1810
1811 return 0;
1812 }
1813
1814
1815
1816
1817
1818
1819
1820
1821
1822 static int vfe_get_selection(struct v4l2_subdev *sd,
1823 struct v4l2_subdev_pad_config *cfg,
1824 struct v4l2_subdev_selection *sel)
1825 {
1826 struct vfe_line *line = v4l2_get_subdevdata(sd);
1827 struct v4l2_subdev_format fmt = { 0 };
1828 struct v4l2_rect *rect;
1829 int ret;
1830
1831 if (line->id != VFE_LINE_PIX)
1832 return -EINVAL;
1833
1834 if (sel->pad == MSM_VFE_PAD_SINK)
1835 switch (sel->target) {
1836 case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1837 fmt.pad = sel->pad;
1838 fmt.which = sel->which;
1839 ret = vfe_get_format(sd, cfg, &fmt);
1840 if (ret < 0)
1841 return ret;
1842
1843 sel->r.left = 0;
1844 sel->r.top = 0;
1845 sel->r.width = fmt.format.width;
1846 sel->r.height = fmt.format.height;
1847 break;
1848 case V4L2_SEL_TGT_COMPOSE:
1849 rect = __vfe_get_compose(line, cfg, sel->which);
1850 if (rect == NULL)
1851 return -EINVAL;
1852
1853 sel->r = *rect;
1854 break;
1855 default:
1856 return -EINVAL;
1857 }
1858 else if (sel->pad == MSM_VFE_PAD_SRC)
1859 switch (sel->target) {
1860 case V4L2_SEL_TGT_CROP_BOUNDS:
1861 rect = __vfe_get_compose(line, cfg, sel->which);
1862 if (rect == NULL)
1863 return -EINVAL;
1864
1865 sel->r.left = rect->left;
1866 sel->r.top = rect->top;
1867 sel->r.width = rect->width;
1868 sel->r.height = rect->height;
1869 break;
1870 case V4L2_SEL_TGT_CROP:
1871 rect = __vfe_get_crop(line, cfg, sel->which);
1872 if (rect == NULL)
1873 return -EINVAL;
1874
1875 sel->r = *rect;
1876 break;
1877 default:
1878 return -EINVAL;
1879 }
1880
1881 return 0;
1882 }
1883
1884
1885
1886
1887
1888
1889
1890
1891
1892 static int vfe_set_selection(struct v4l2_subdev *sd,
1893 struct v4l2_subdev_pad_config *cfg,
1894 struct v4l2_subdev_selection *sel)
1895 {
1896 struct vfe_line *line = v4l2_get_subdevdata(sd);
1897 struct v4l2_rect *rect;
1898 int ret;
1899
1900 if (line->id != VFE_LINE_PIX)
1901 return -EINVAL;
1902
1903 if (sel->target == V4L2_SEL_TGT_COMPOSE &&
1904 sel->pad == MSM_VFE_PAD_SINK) {
1905 struct v4l2_subdev_selection crop = { 0 };
1906
1907 rect = __vfe_get_compose(line, cfg, sel->which);
1908 if (rect == NULL)
1909 return -EINVAL;
1910
1911 vfe_try_compose(line, cfg, &sel->r, sel->which);
1912 *rect = sel->r;
1913
1914
1915 crop.which = sel->which;
1916 crop.pad = MSM_VFE_PAD_SRC;
1917 crop.target = V4L2_SEL_TGT_CROP;
1918 crop.r = *rect;
1919 ret = vfe_set_selection(sd, cfg, &crop);
1920 } else if (sel->target == V4L2_SEL_TGT_CROP &&
1921 sel->pad == MSM_VFE_PAD_SRC) {
1922 struct v4l2_subdev_format fmt = { 0 };
1923
1924 rect = __vfe_get_crop(line, cfg, sel->which);
1925 if (rect == NULL)
1926 return -EINVAL;
1927
1928 vfe_try_crop(line, cfg, &sel->r, sel->which);
1929 *rect = sel->r;
1930
1931
1932 fmt.which = sel->which;
1933 fmt.pad = MSM_VFE_PAD_SRC;
1934 ret = vfe_get_format(sd, cfg, &fmt);
1935 if (ret < 0)
1936 return ret;
1937
1938 fmt.format.width = rect->width;
1939 fmt.format.height = rect->height;
1940 ret = vfe_set_format(sd, cfg, &fmt);
1941 } else {
1942 ret = -EINVAL;
1943 }
1944
1945 return ret;
1946 }
1947
1948
1949
1950
1951
1952
1953
1954
1955
1956
1957 static int vfe_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1958 {
1959 struct v4l2_subdev_format format = {
1960 .pad = MSM_VFE_PAD_SINK,
1961 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
1962 V4L2_SUBDEV_FORMAT_ACTIVE,
1963 .format = {
1964 .code = MEDIA_BUS_FMT_UYVY8_2X8,
1965 .width = 1920,
1966 .height = 1080
1967 }
1968 };
1969
1970 return vfe_set_format(sd, fh ? fh->pad : NULL, &format);
1971 }
1972
1973
1974
1975
1976
1977
1978
1979
1980 int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
1981 const struct resources *res, u8 id)
1982 {
1983 struct device *dev = camss->dev;
1984 struct platform_device *pdev = to_platform_device(dev);
1985 struct resource *r;
1986 int i, j;
1987 int ret;
1988
1989 vfe->isr_ops.reset_ack = vfe_isr_reset_ack;
1990 vfe->isr_ops.halt_ack = vfe_isr_halt_ack;
1991 vfe->isr_ops.reg_update = vfe_isr_reg_update;
1992 vfe->isr_ops.sof = vfe_isr_sof;
1993 vfe->isr_ops.comp_done = vfe_isr_comp_done;
1994 vfe->isr_ops.wm_done = vfe_isr_wm_done;
1995
1996 if (camss->version == CAMSS_8x16)
1997 vfe->ops = &vfe_ops_4_1;
1998 else if (camss->version == CAMSS_8x96)
1999 vfe->ops = &vfe_ops_4_7;
2000 else
2001 return -EINVAL;
2002
2003
2004
2005 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
2006 vfe->base = devm_ioremap_resource(dev, r);
2007 if (IS_ERR(vfe->base)) {
2008 dev_err(dev, "could not map memory\n");
2009 return PTR_ERR(vfe->base);
2010 }
2011
2012
2013
2014 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
2015 res->interrupt[0]);
2016 if (!r) {
2017 dev_err(dev, "missing IRQ\n");
2018 return -EINVAL;
2019 }
2020
2021 vfe->irq = r->start;
2022 snprintf(vfe->irq_name, sizeof(vfe->irq_name), "%s_%s%d",
2023 dev_name(dev), MSM_VFE_NAME, vfe->id);
2024 ret = devm_request_irq(dev, vfe->irq, vfe->ops->isr,
2025 IRQF_TRIGGER_RISING, vfe->irq_name, vfe);
2026 if (ret < 0) {
2027 dev_err(dev, "request_irq failed: %d\n", ret);
2028 return ret;
2029 }
2030
2031
2032
2033 vfe->nclocks = 0;
2034 while (res->clock[vfe->nclocks])
2035 vfe->nclocks++;
2036
2037 vfe->clock = devm_kcalloc(dev, vfe->nclocks, sizeof(*vfe->clock),
2038 GFP_KERNEL);
2039 if (!vfe->clock)
2040 return -ENOMEM;
2041
2042 for (i = 0; i < vfe->nclocks; i++) {
2043 struct camss_clock *clock = &vfe->clock[i];
2044
2045 clock->clk = devm_clk_get(dev, res->clock[i]);
2046 if (IS_ERR(clock->clk))
2047 return PTR_ERR(clock->clk);
2048
2049 clock->name = res->clock[i];
2050
2051 clock->nfreqs = 0;
2052 while (res->clock_rate[i][clock->nfreqs])
2053 clock->nfreqs++;
2054
2055 if (!clock->nfreqs) {
2056 clock->freq = NULL;
2057 continue;
2058 }
2059
2060 clock->freq = devm_kcalloc(dev,
2061 clock->nfreqs,
2062 sizeof(*clock->freq),
2063 GFP_KERNEL);
2064 if (!clock->freq)
2065 return -ENOMEM;
2066
2067 for (j = 0; j < clock->nfreqs; j++)
2068 clock->freq[j] = res->clock_rate[i][j];
2069 }
2070
2071 mutex_init(&vfe->power_lock);
2072 vfe->power_count = 0;
2073
2074 mutex_init(&vfe->stream_lock);
2075 vfe->stream_count = 0;
2076
2077 spin_lock_init(&vfe->output_lock);
2078
2079 vfe->camss = camss;
2080 vfe->id = id;
2081 vfe->reg_update = 0;
2082
2083 for (i = VFE_LINE_RDI0; i <= VFE_LINE_PIX; i++) {
2084 struct vfe_line *l = &vfe->line[i];
2085
2086 l->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
2087 l->video_out.camss = camss;
2088 l->id = i;
2089 init_completion(&l->output.sof);
2090 init_completion(&l->output.reg_update);
2091
2092 if (camss->version == CAMSS_8x16) {
2093 if (i == VFE_LINE_PIX) {
2094 l->formats = formats_pix_8x16;
2095 l->nformats = ARRAY_SIZE(formats_pix_8x16);
2096 } else {
2097 l->formats = formats_rdi_8x16;
2098 l->nformats = ARRAY_SIZE(formats_rdi_8x16);
2099 }
2100 } else if (camss->version == CAMSS_8x96) {
2101 if (i == VFE_LINE_PIX) {
2102 l->formats = formats_pix_8x96;
2103 l->nformats = ARRAY_SIZE(formats_pix_8x96);
2104 } else {
2105 l->formats = formats_rdi_8x96;
2106 l->nformats = ARRAY_SIZE(formats_rdi_8x96);
2107 }
2108 } else {
2109 return -EINVAL;
2110 }
2111 }
2112
2113 init_completion(&vfe->reset_complete);
2114 init_completion(&vfe->halt_complete);
2115
2116 return 0;
2117 }
2118
2119
2120
2121
2122
2123
2124 void msm_vfe_get_vfe_id(struct media_entity *entity, u8 *id)
2125 {
2126 struct v4l2_subdev *sd;
2127 struct vfe_line *line;
2128 struct vfe_device *vfe;
2129
2130 sd = media_entity_to_v4l2_subdev(entity);
2131 line = v4l2_get_subdevdata(sd);
2132 vfe = to_vfe(line);
2133
2134 *id = vfe->id;
2135 }
2136
2137
2138
2139
2140
2141
2142 void msm_vfe_get_vfe_line_id(struct media_entity *entity, enum vfe_line_id *id)
2143 {
2144 struct v4l2_subdev *sd;
2145 struct vfe_line *line;
2146
2147 sd = media_entity_to_v4l2_subdev(entity);
2148 line = v4l2_get_subdevdata(sd);
2149
2150 *id = line->id;
2151 }
2152
2153
2154
2155
2156
2157
2158
2159
2160
2161
2162 static int vfe_link_setup(struct media_entity *entity,
2163 const struct media_pad *local,
2164 const struct media_pad *remote, u32 flags)
2165 {
2166 if (flags & MEDIA_LNK_FL_ENABLED)
2167 if (media_entity_remote_pad(local))
2168 return -EBUSY;
2169
2170 return 0;
2171 }
2172
2173 static const struct v4l2_subdev_core_ops vfe_core_ops = {
2174 .s_power = vfe_set_power,
2175 };
2176
2177 static const struct v4l2_subdev_video_ops vfe_video_ops = {
2178 .s_stream = vfe_set_stream,
2179 };
2180
2181 static const struct v4l2_subdev_pad_ops vfe_pad_ops = {
2182 .enum_mbus_code = vfe_enum_mbus_code,
2183 .enum_frame_size = vfe_enum_frame_size,
2184 .get_fmt = vfe_get_format,
2185 .set_fmt = vfe_set_format,
2186 .get_selection = vfe_get_selection,
2187 .set_selection = vfe_set_selection,
2188 };
2189
2190 static const struct v4l2_subdev_ops vfe_v4l2_ops = {
2191 .core = &vfe_core_ops,
2192 .video = &vfe_video_ops,
2193 .pad = &vfe_pad_ops,
2194 };
2195
2196 static const struct v4l2_subdev_internal_ops vfe_v4l2_internal_ops = {
2197 .open = vfe_init_formats,
2198 };
2199
2200 static const struct media_entity_operations vfe_media_ops = {
2201 .link_setup = vfe_link_setup,
2202 .link_validate = v4l2_subdev_link_validate,
2203 };
2204
2205 static const struct camss_video_ops camss_vfe_video_ops = {
2206 .queue_buffer = vfe_queue_buffer,
2207 .flush_buffers = vfe_flush_buffers,
2208 };
2209
2210 void msm_vfe_stop_streaming(struct vfe_device *vfe)
2211 {
2212 int i;
2213
2214 for (i = 0; i < ARRAY_SIZE(vfe->line); i++)
2215 msm_video_stop_streaming(&vfe->line[i].video_out);
2216 }
2217
2218
2219
2220
2221
2222
2223
2224
2225
2226
2227
2228
2229
2230 int msm_vfe_register_entities(struct vfe_device *vfe,
2231 struct v4l2_device *v4l2_dev)
2232 {
2233 struct device *dev = vfe->camss->dev;
2234 struct v4l2_subdev *sd;
2235 struct media_pad *pads;
2236 struct camss_video *video_out;
2237 int ret;
2238 int i;
2239
2240 for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
2241 char name[32];
2242
2243 sd = &vfe->line[i].subdev;
2244 pads = vfe->line[i].pads;
2245 video_out = &vfe->line[i].video_out;
2246
2247 v4l2_subdev_init(sd, &vfe_v4l2_ops);
2248 sd->internal_ops = &vfe_v4l2_internal_ops;
2249 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
2250 if (i == VFE_LINE_PIX)
2251 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
2252 MSM_VFE_NAME, vfe->id, "pix");
2253 else
2254 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s%d",
2255 MSM_VFE_NAME, vfe->id, "rdi", i);
2256
2257 v4l2_set_subdevdata(sd, &vfe->line[i]);
2258
2259 ret = vfe_init_formats(sd, NULL);
2260 if (ret < 0) {
2261 dev_err(dev, "Failed to init format: %d\n", ret);
2262 goto error_init;
2263 }
2264
2265 pads[MSM_VFE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
2266 pads[MSM_VFE_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
2267
2268 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
2269 sd->entity.ops = &vfe_media_ops;
2270 ret = media_entity_pads_init(&sd->entity, MSM_VFE_PADS_NUM,
2271 pads);
2272 if (ret < 0) {
2273 dev_err(dev, "Failed to init media entity: %d\n", ret);
2274 goto error_init;
2275 }
2276
2277 ret = v4l2_device_register_subdev(v4l2_dev, sd);
2278 if (ret < 0) {
2279 dev_err(dev, "Failed to register subdev: %d\n", ret);
2280 goto error_reg_subdev;
2281 }
2282
2283 video_out->ops = &camss_vfe_video_ops;
2284 video_out->bpl_alignment = 8;
2285 video_out->line_based = 0;
2286 if (i == VFE_LINE_PIX) {
2287 video_out->bpl_alignment = 16;
2288 video_out->line_based = 1;
2289 }
2290 snprintf(name, ARRAY_SIZE(name), "%s%d_%s%d",
2291 MSM_VFE_NAME, vfe->id, "video", i);
2292 ret = msm_video_register(video_out, v4l2_dev, name,
2293 i == VFE_LINE_PIX ? 1 : 0);
2294 if (ret < 0) {
2295 dev_err(dev, "Failed to register video node: %d\n",
2296 ret);
2297 goto error_reg_video;
2298 }
2299
2300 ret = media_create_pad_link(
2301 &sd->entity, MSM_VFE_PAD_SRC,
2302 &video_out->vdev.entity, 0,
2303 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
2304 if (ret < 0) {
2305 dev_err(dev, "Failed to link %s->%s entities: %d\n",
2306 sd->entity.name, video_out->vdev.entity.name,
2307 ret);
2308 goto error_link;
2309 }
2310 }
2311
2312 return 0;
2313
2314 error_link:
2315 msm_video_unregister(video_out);
2316
2317 error_reg_video:
2318 v4l2_device_unregister_subdev(sd);
2319
2320 error_reg_subdev:
2321 media_entity_cleanup(&sd->entity);
2322
2323 error_init:
2324 for (i--; i >= 0; i--) {
2325 sd = &vfe->line[i].subdev;
2326 video_out = &vfe->line[i].video_out;
2327
2328 msm_video_unregister(video_out);
2329 v4l2_device_unregister_subdev(sd);
2330 media_entity_cleanup(&sd->entity);
2331 }
2332
2333 return ret;
2334 }
2335
2336
2337
2338
2339
2340 void msm_vfe_unregister_entities(struct vfe_device *vfe)
2341 {
2342 int i;
2343
2344 mutex_destroy(&vfe->power_lock);
2345 mutex_destroy(&vfe->stream_lock);
2346
2347 for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
2348 struct v4l2_subdev *sd = &vfe->line[i].subdev;
2349 struct camss_video *video_out = &vfe->line[i].video_out;
2350
2351 msm_video_unregister(video_out);
2352 v4l2_device_unregister_subdev(sd);
2353 media_entity_cleanup(&sd->entity);
2354 }
2355 }