1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * camss-vfe-gen1.c
4  *
5  * Qualcomm MSM Camera Subsystem - VFE Common functionality for Gen 1 versions of hw (4.1, 4.7..)
6  *
7  * Copyright (C) 2020 Linaro Ltd.
8  */
9 
10 #include "camss.h"
11 #include "camss-vfe.h"
12 #include "camss-vfe-gen1.h"
13 
14 /* Max number of frame drop updates per frame */
15 #define VFE_FRAME_DROP_UPDATES 2
16 #define VFE_NEXT_SOF_MS 500
17 
18 int vfe_gen1_halt(struct vfe_device *vfe)
19 {
20 	unsigned long time;
21 
22 	reinit_completion(&vfe->halt_complete);
23 
24 	vfe->ops_gen1->halt_request(vfe);
25 
26 	time = wait_for_completion_timeout(&vfe->halt_complete,
27 					   msecs_to_jiffies(VFE_HALT_TIMEOUT_MS));
28 	if (!time) {
29 		dev_err(vfe->camss->dev, "VFE halt timeout\n");
30 		return -EIO;
31 	}
32 
33 	return 0;
34 }
35 
36 static int vfe_disable_output(struct vfe_line *line)
37 {
38 	struct vfe_device *vfe = to_vfe(line);
39 	struct vfe_output *output = &line->output;
40 	const struct vfe_hw_ops *ops = vfe->ops;
41 	unsigned long flags;
42 	unsigned long time;
43 	unsigned int i;
44 
45 	spin_lock_irqsave(&vfe->output_lock, flags);
46 
47 	output->gen1.wait_sof = 1;
48 	spin_unlock_irqrestore(&vfe->output_lock, flags);
49 
50 	time = wait_for_completion_timeout(&output->sof, msecs_to_jiffies(VFE_NEXT_SOF_MS));
51 	if (!time)
52 		dev_err(vfe->camss->dev, "VFE sof timeout\n");
53 
54 	spin_lock_irqsave(&vfe->output_lock, flags);
55 	for (i = 0; i < output->wm_num; i++)
56 		vfe->ops_gen1->wm_enable(vfe, output->wm_idx[i], 0);
57 
58 	ops->reg_update(vfe, line->id);
59 	output->wait_reg_update = 1;
60 	spin_unlock_irqrestore(&vfe->output_lock, flags);
61 
62 	time = wait_for_completion_timeout(&output->reg_update, msecs_to_jiffies(VFE_NEXT_SOF_MS));
63 	if (!time)
64 		dev_err(vfe->camss->dev, "VFE reg update timeout\n");
65 
66 	spin_lock_irqsave(&vfe->output_lock, flags);
67 
68 	if (line->id != VFE_LINE_PIX) {
69 		vfe->ops_gen1->wm_frame_based(vfe, output->wm_idx[0], 0);
70 		vfe->ops_gen1->bus_disconnect_wm_from_rdi(vfe, output->wm_idx[0], line->id);
71 		vfe->ops_gen1->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 0);
72 		vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[0], 0);
73 		spin_unlock_irqrestore(&vfe->output_lock, flags);
74 	} else {
75 		for (i = 0; i < output->wm_num; i++) {
76 			vfe->ops_gen1->wm_line_based(vfe, output->wm_idx[i], NULL, i, 0);
77 			vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[i], 0);
78 		}
79 
80 		vfe->ops_gen1->enable_irq_pix_line(vfe, 0, line->id, 0);
81 		vfe->ops_gen1->set_module_cfg(vfe, 0);
82 		vfe->ops_gen1->set_realign_cfg(vfe, line, 0);
83 		vfe->ops_gen1->set_xbar_cfg(vfe, output, 0);
84 		vfe->ops_gen1->set_camif_cmd(vfe, 0);
85 
86 		spin_unlock_irqrestore(&vfe->output_lock, flags);
87 
88 		vfe->ops_gen1->camif_wait_for_stop(vfe, vfe->camss->dev);
89 	}
90 
91 	return 0;
92 }
93 
94 /*
95  * vfe_gen1_disable - Disable streaming on VFE line
96  * @line: VFE line
97  *
98  * Return 0 on success or a negative error code otherwise
99  */
100 int vfe_gen1_disable(struct vfe_line *line)
101 {
102 	struct vfe_device *vfe = to_vfe(line);
103 
104 	vfe_disable_output(line);
105 
106 	vfe_put_output(line);
107 
108 	mutex_lock(&vfe->stream_lock);
109 
110 	if (vfe->stream_count == 1)
111 		vfe->ops_gen1->bus_enable_wr_if(vfe, 0);
112 
113 	vfe->stream_count--;
114 
115 	mutex_unlock(&vfe->stream_lock);
116 
117 	return 0;
118 }
119 
120 static void vfe_output_init_addrs(struct vfe_device *vfe,
121 				  struct vfe_output *output, u8 sync,
122 				  struct vfe_line *line)
123 {
124 	u32 ping_addr;
125 	u32 pong_addr;
126 	unsigned int i;
127 
128 	output->gen1.active_buf = 0;
129 
130 	for (i = 0; i < output->wm_num; i++) {
131 		if (output->buf[0])
132 			ping_addr = output->buf[0]->addr[i];
133 		else
134 			ping_addr = 0;
135 
136 		if (output->buf[1])
137 			pong_addr = output->buf[1]->addr[i];
138 		else
139 			pong_addr = ping_addr;
140 
141 		vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], ping_addr);
142 		vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], pong_addr);
143 		if (sync)
144 			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
145 	}
146 }
147 
148 static void vfe_output_frame_drop(struct vfe_device *vfe,
149 				  struct vfe_output *output,
150 				  u32 drop_pattern)
151 {
152 	u8 drop_period;
153 	unsigned int i;
154 
155 	/* We need to toggle update period to be valid on next frame */
156 	output->drop_update_idx++;
157 	output->drop_update_idx %= VFE_FRAME_DROP_UPDATES;
158 	drop_period = VFE_FRAME_DROP_VAL + output->drop_update_idx;
159 
160 	for (i = 0; i < output->wm_num; i++) {
161 		vfe->ops_gen1->wm_set_framedrop_period(vfe, output->wm_idx[i], drop_period);
162 		vfe->ops_gen1->wm_set_framedrop_pattern(vfe, output->wm_idx[i], drop_pattern);
163 	}
164 
165 	vfe->ops->reg_update(vfe, container_of(output, struct vfe_line, output)->id);
166 }
167 
168 static int vfe_enable_output(struct vfe_line *line)
169 {
170 	struct vfe_device *vfe = to_vfe(line);
171 	struct vfe_output *output = &line->output;
172 	const struct vfe_hw_ops *ops = vfe->ops;
173 	struct media_entity *sensor;
174 	unsigned long flags;
175 	unsigned int frame_skip = 0;
176 	unsigned int i;
177 	u16 ub_size;
178 
179 	ub_size = vfe->ops_gen1->get_ub_size(vfe->id);
180 	if (!ub_size)
181 		return -EINVAL;
182 
183 	sensor = camss_find_sensor(&line->subdev.entity);
184 	if (sensor) {
185 		struct v4l2_subdev *subdev = media_entity_to_v4l2_subdev(sensor);
186 
187 		v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
188 		/* Max frame skip is 29 frames */
189 		if (frame_skip > VFE_FRAME_DROP_VAL - 1)
190 			frame_skip = VFE_FRAME_DROP_VAL - 1;
191 	}
192 
193 	spin_lock_irqsave(&vfe->output_lock, flags);
194 
195 	ops->reg_update_clear(vfe, line->id);
196 
197 	if (output->state != VFE_OUTPUT_RESERVED) {
198 		dev_err(vfe->camss->dev, "Output is not in reserved state %d\n", output->state);
199 		spin_unlock_irqrestore(&vfe->output_lock, flags);
200 		return -EINVAL;
201 	}
202 	output->state = VFE_OUTPUT_IDLE;
203 
204 	output->buf[0] = vfe_buf_get_pending(output);
205 	output->buf[1] = vfe_buf_get_pending(output);
206 
207 	if (!output->buf[0] && output->buf[1]) {
208 		output->buf[0] = output->buf[1];
209 		output->buf[1] = NULL;
210 	}
211 
212 	if (output->buf[0])
213 		output->state = VFE_OUTPUT_SINGLE;
214 
215 	if (output->buf[1])
216 		output->state = VFE_OUTPUT_CONTINUOUS;
217 
218 	switch (output->state) {
219 	case VFE_OUTPUT_SINGLE:
220 		vfe_output_frame_drop(vfe, output, 1 << frame_skip);
221 		break;
222 	case VFE_OUTPUT_CONTINUOUS:
223 		vfe_output_frame_drop(vfe, output, 3 << frame_skip);
224 		break;
225 	default:
226 		vfe_output_frame_drop(vfe, output, 0);
227 		break;
228 	}
229 
230 	output->sequence = 0;
231 	output->gen1.wait_sof = 0;
232 	output->wait_reg_update = 0;
233 	reinit_completion(&output->sof);
234 	reinit_completion(&output->reg_update);
235 
236 	vfe_output_init_addrs(vfe, output, 0, line);
237 
238 	if (line->id != VFE_LINE_PIX) {
239 		vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[0], 1);
240 		vfe->ops_gen1->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 1);
241 		vfe->ops_gen1->bus_connect_wm_to_rdi(vfe, output->wm_idx[0], line->id);
242 		vfe->ops_gen1->wm_set_subsample(vfe, output->wm_idx[0]);
243 		vfe->ops_gen1->set_rdi_cid(vfe, line->id, 0);
244 		vfe->ops_gen1->wm_set_ub_cfg(vfe, output->wm_idx[0],
245 					    (ub_size + 1) * output->wm_idx[0], ub_size);
246 		vfe->ops_gen1->wm_frame_based(vfe, output->wm_idx[0], 1);
247 		vfe->ops_gen1->wm_enable(vfe, output->wm_idx[0], 1);
248 		vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[0]);
249 	} else {
250 		ub_size /= output->wm_num;
251 		for (i = 0; i < output->wm_num; i++) {
252 			vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[i], 1);
253 			vfe->ops_gen1->wm_set_subsample(vfe, output->wm_idx[i]);
254 			vfe->ops_gen1->wm_set_ub_cfg(vfe, output->wm_idx[i],
255 						     (ub_size + 1) * output->wm_idx[i], ub_size);
256 			vfe->ops_gen1->wm_line_based(vfe, output->wm_idx[i],
257 						     &line->video_out.active_fmt.fmt.pix_mp, i, 1);
258 			vfe->ops_gen1->wm_enable(vfe, output->wm_idx[i], 1);
259 			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
260 		}
261 		vfe->ops_gen1->enable_irq_pix_line(vfe, 0, line->id, 1);
262 		vfe->ops_gen1->set_module_cfg(vfe, 1);
263 		vfe->ops_gen1->set_camif_cfg(vfe, line);
264 		vfe->ops_gen1->set_realign_cfg(vfe, line, 1);
265 		vfe->ops_gen1->set_xbar_cfg(vfe, output, 1);
266 		vfe->ops_gen1->set_demux_cfg(vfe, line);
267 		vfe->ops_gen1->set_scale_cfg(vfe, line);
268 		vfe->ops_gen1->set_crop_cfg(vfe, line);
269 		vfe->ops_gen1->set_clamp_cfg(vfe);
270 		vfe->ops_gen1->set_camif_cmd(vfe, 1);
271 	}
272 
273 	ops->reg_update(vfe, line->id);
274 
275 	spin_unlock_irqrestore(&vfe->output_lock, flags);
276 
277 	return 0;
278 }
279 
280 static int vfe_get_output(struct vfe_line *line)
281 {
282 	struct vfe_device *vfe = to_vfe(line);
283 	struct vfe_output *output;
284 	struct v4l2_format *f = &line->video_out.active_fmt;
285 	unsigned long flags;
286 	int i;
287 	int wm_idx;
288 
289 	spin_lock_irqsave(&vfe->output_lock, flags);
290 
291 	output = &line->output;
292 	if (output->state != VFE_OUTPUT_OFF) {
293 		dev_err(vfe->camss->dev, "Output is running\n");
294 		goto error;
295 	}
296 	output->state = VFE_OUTPUT_RESERVED;
297 
298 	output->gen1.active_buf = 0;
299 
300 	switch (f->fmt.pix_mp.pixelformat) {
301 	case V4L2_PIX_FMT_NV12:
302 	case V4L2_PIX_FMT_NV21:
303 	case V4L2_PIX_FMT_NV16:
304 	case V4L2_PIX_FMT_NV61:
305 		output->wm_num = 2;
306 		break;
307 	default:
308 		output->wm_num = 1;
309 		break;
310 	}
311 
312 	for (i = 0; i < output->wm_num; i++) {
313 		wm_idx = vfe_reserve_wm(vfe, line->id);
314 		if (wm_idx < 0) {
315 			dev_err(vfe->camss->dev, "Can not reserve wm\n");
316 			goto error_get_wm;
317 		}
318 		output->wm_idx[i] = wm_idx;
319 	}
320 
321 	output->drop_update_idx = 0;
322 
323 	spin_unlock_irqrestore(&vfe->output_lock, flags);
324 
325 	return 0;
326 
327 error_get_wm:
328 	for (i--; i >= 0; i--)
329 		vfe_release_wm(vfe, output->wm_idx[i]);
330 	output->state = VFE_OUTPUT_OFF;
331 error:
332 	spin_unlock_irqrestore(&vfe->output_lock, flags);
333 
334 	return -EINVAL;
335 }
336 
337 int vfe_gen1_enable(struct vfe_line *line)
338 {
339 	struct vfe_device *vfe = to_vfe(line);
340 	int ret;
341 
342 	mutex_lock(&vfe->stream_lock);
343 
344 	if (!vfe->stream_count) {
345 		vfe->ops_gen1->enable_irq_common(vfe);
346 		vfe->ops_gen1->bus_enable_wr_if(vfe, 1);
347 		vfe->ops_gen1->set_qos(vfe);
348 		vfe->ops_gen1->set_ds(vfe);
349 	}
350 
351 	vfe->stream_count++;
352 
353 	mutex_unlock(&vfe->stream_lock);
354 
355 	ret = vfe_get_output(line);
356 	if (ret < 0)
357 		goto error_get_output;
358 
359 	ret = vfe_enable_output(line);
360 	if (ret < 0)
361 		goto error_enable_output;
362 
363 	vfe->was_streaming = 1;
364 
365 	return 0;
366 
367 error_enable_output:
368 	vfe_put_output(line);
369 
370 error_get_output:
371 	mutex_lock(&vfe->stream_lock);
372 
373 	if (vfe->stream_count == 1)
374 		vfe->ops_gen1->bus_enable_wr_if(vfe, 0);
375 
376 	vfe->stream_count--;
377 
378 	mutex_unlock(&vfe->stream_lock);
379 
380 	return ret;
381 }
382 
383 static void vfe_output_update_ping_addr(struct vfe_device *vfe,
384 					struct vfe_output *output, u8 sync,
385 					struct vfe_line *line)
386 {
387 	u32 addr;
388 	unsigned int i;
389 
390 	for (i = 0; i < output->wm_num; i++) {
391 		if (output->buf[0])
392 			addr = output->buf[0]->addr[i];
393 		else
394 			addr = 0;
395 
396 		vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], addr);
397 		if (sync)
398 			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
399 	}
400 }
401 
402 static void vfe_output_update_pong_addr(struct vfe_device *vfe,
403 					struct vfe_output *output, u8 sync,
404 					struct vfe_line *line)
405 {
406 	u32 addr;
407 	unsigned int i;
408 
409 	for (i = 0; i < output->wm_num; i++) {
410 		if (output->buf[1])
411 			addr = output->buf[1]->addr[i];
412 		else
413 			addr = 0;
414 
415 		vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], addr);
416 		if (sync)
417 			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
418 	}
419 }
420 
421 static void vfe_buf_update_wm_on_next(struct vfe_device *vfe,
422 				      struct vfe_output *output)
423 {
424 	switch (output->state) {
425 	case VFE_OUTPUT_CONTINUOUS:
426 		vfe_output_frame_drop(vfe, output, 3);
427 		break;
428 	case VFE_OUTPUT_SINGLE:
429 	default:
430 		dev_err_ratelimited(vfe->camss->dev,
431 				    "Next buf in wrong state! %d\n",
432 				    output->state);
433 		break;
434 	}
435 }
436 
437 static void vfe_buf_update_wm_on_last(struct vfe_device *vfe,
438 				      struct vfe_output *output)
439 {
440 	switch (output->state) {
441 	case VFE_OUTPUT_CONTINUOUS:
442 		output->state = VFE_OUTPUT_SINGLE;
443 		vfe_output_frame_drop(vfe, output, 1);
444 		break;
445 	case VFE_OUTPUT_SINGLE:
446 		output->state = VFE_OUTPUT_STOPPING;
447 		vfe_output_frame_drop(vfe, output, 0);
448 		break;
449 	default:
450 		dev_err_ratelimited(vfe->camss->dev,
451 				    "Last buff in wrong state! %d\n",
452 				    output->state);
453 		break;
454 	}
455 }
456 
457 static void vfe_buf_update_wm_on_new(struct vfe_device *vfe,
458 				     struct vfe_output *output,
459 				     struct camss_buffer *new_buf,
460 				     struct vfe_line *line)
461 {
462 	int inactive_idx;
463 
464 	switch (output->state) {
465 	case VFE_OUTPUT_SINGLE:
466 		inactive_idx = !output->gen1.active_buf;
467 
468 		if (!output->buf[inactive_idx]) {
469 			output->buf[inactive_idx] = new_buf;
470 
471 			if (inactive_idx)
472 				vfe_output_update_pong_addr(vfe, output, 0, line);
473 			else
474 				vfe_output_update_ping_addr(vfe, output, 0, line);
475 
476 			vfe_output_frame_drop(vfe, output, 3);
477 			output->state = VFE_OUTPUT_CONTINUOUS;
478 		} else {
479 			vfe_buf_add_pending(output, new_buf);
480 			dev_err_ratelimited(vfe->camss->dev,
481 					    "Inactive buffer is busy\n");
482 		}
483 		break;
484 
485 	case VFE_OUTPUT_IDLE:
486 		if (!output->buf[0]) {
487 			output->buf[0] = new_buf;
488 
489 			vfe_output_init_addrs(vfe, output, 1, line);
490 			vfe_output_frame_drop(vfe, output, 1);
491 
492 			output->state = VFE_OUTPUT_SINGLE;
493 		} else {
494 			vfe_buf_add_pending(output, new_buf);
495 			dev_err_ratelimited(vfe->camss->dev,
496 					    "Output idle with buffer set!\n");
497 		}
498 		break;
499 
500 	case VFE_OUTPUT_CONTINUOUS:
501 	default:
502 		vfe_buf_add_pending(output, new_buf);
503 		break;
504 	}
505 }
506 
507 /*
508  * vfe_isr_halt_ack - Process halt ack
509  * @vfe: VFE Device
510  */
511 static void vfe_isr_halt_ack(struct vfe_device *vfe)
512 {
513 	complete(&vfe->halt_complete);
514 	vfe->ops_gen1->halt_clear(vfe);
515 }
516 
517 /*
518  * vfe_isr_sof - Process start of frame interrupt
519  * @vfe: VFE Device
520  * @line_id: VFE line
521  */
522 static void vfe_isr_sof(struct vfe_device *vfe, enum vfe_line_id line_id)
523 {
524 	struct vfe_output *output;
525 	unsigned long flags;
526 
527 	spin_lock_irqsave(&vfe->output_lock, flags);
528 	output = &vfe->line[line_id].output;
529 	if (output->gen1.wait_sof) {
530 		output->gen1.wait_sof = 0;
531 		complete(&output->sof);
532 	}
533 	spin_unlock_irqrestore(&vfe->output_lock, flags);
534 }
535 
536 /*
537  * vfe_isr_reg_update - Process reg update interrupt
538  * @vfe: VFE Device
539  * @line_id: VFE line
540  */
541 static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
542 {
543 	struct vfe_output *output;
544 	struct vfe_line *line = &vfe->line[line_id];
545 	unsigned long flags;
546 
547 	spin_lock_irqsave(&vfe->output_lock, flags);
548 	vfe->ops->reg_update_clear(vfe, line_id);
549 
550 	output = &line->output;
551 
552 	if (output->wait_reg_update) {
553 		output->wait_reg_update = 0;
554 		complete(&output->reg_update);
555 		spin_unlock_irqrestore(&vfe->output_lock, flags);
556 		return;
557 	}
558 
559 	if (output->state == VFE_OUTPUT_STOPPING) {
560 		/* Release last buffer when hw is idle */
561 		if (output->last_buffer) {
562 			vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
563 					VB2_BUF_STATE_DONE);
564 			output->last_buffer = NULL;
565 		}
566 		output->state = VFE_OUTPUT_IDLE;
567 
568 		/* Buffers received in stopping state are queued in */
569 		/* dma pending queue, start next capture here */
570 
571 		output->buf[0] = vfe_buf_get_pending(output);
572 		output->buf[1] = vfe_buf_get_pending(output);
573 
574 		if (!output->buf[0] && output->buf[1]) {
575 			output->buf[0] = output->buf[1];
576 			output->buf[1] = NULL;
577 		}
578 
579 		if (output->buf[0])
580 			output->state = VFE_OUTPUT_SINGLE;
581 
582 		if (output->buf[1])
583 			output->state = VFE_OUTPUT_CONTINUOUS;
584 
585 		switch (output->state) {
586 		case VFE_OUTPUT_SINGLE:
587 			vfe_output_frame_drop(vfe, output, 2);
588 			break;
589 		case VFE_OUTPUT_CONTINUOUS:
590 			vfe_output_frame_drop(vfe, output, 3);
591 			break;
592 		default:
593 			vfe_output_frame_drop(vfe, output, 0);
594 			break;
595 		}
596 
597 		vfe_output_init_addrs(vfe, output, 1, &vfe->line[line_id]);
598 	}
599 
600 	spin_unlock_irqrestore(&vfe->output_lock, flags);
601 }
602 
603 /*
604  * vfe_isr_wm_done - Process write master done interrupt
605  * @vfe: VFE Device
606  * @wm: Write master id
607  */
608 static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
609 {
610 	struct camss_buffer *ready_buf;
611 	struct vfe_output *output;
612 	dma_addr_t *new_addr;
613 	unsigned long flags;
614 	u32 active_index;
615 	u64 ts = ktime_get_ns();
616 	unsigned int i;
617 
618 	active_index = vfe->ops_gen1->wm_get_ping_pong_status(vfe, wm);
619 
620 	spin_lock_irqsave(&vfe->output_lock, flags);
621 
622 	if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
623 		dev_err_ratelimited(vfe->camss->dev,
624 				    "Received wm done for unmapped index\n");
625 		goto out_unlock;
626 	}
627 	output = &vfe->line[vfe->wm_output_map[wm]].output;
628 
629 	if (output->gen1.active_buf == active_index && 0) {
630 		dev_err_ratelimited(vfe->camss->dev,
631 				    "Active buffer mismatch!\n");
632 		goto out_unlock;
633 	}
634 	output->gen1.active_buf = active_index;
635 
636 	ready_buf = output->buf[!active_index];
637 	if (!ready_buf) {
638 		dev_err_ratelimited(vfe->camss->dev,
639 				    "Missing ready buf %d %d!\n",
640 				    !active_index, output->state);
641 		goto out_unlock;
642 	}
643 
644 	ready_buf->vb.vb2_buf.timestamp = ts;
645 	ready_buf->vb.sequence = output->sequence++;
646 
647 	/* Get next buffer */
648 	output->buf[!active_index] = vfe_buf_get_pending(output);
649 	if (!output->buf[!active_index]) {
650 		/* No next buffer - set same address */
651 		new_addr = ready_buf->addr;
652 		vfe_buf_update_wm_on_last(vfe, output);
653 	} else {
654 		new_addr = output->buf[!active_index]->addr;
655 		vfe_buf_update_wm_on_next(vfe, output);
656 	}
657 
658 	if (active_index)
659 		for (i = 0; i < output->wm_num; i++)
660 			vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], new_addr[i]);
661 	else
662 		for (i = 0; i < output->wm_num; i++)
663 			vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], new_addr[i]);
664 
665 	spin_unlock_irqrestore(&vfe->output_lock, flags);
666 
667 	if (output->state == VFE_OUTPUT_STOPPING)
668 		output->last_buffer = ready_buf;
669 	else
670 		vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
671 
672 	return;
673 
674 out_unlock:
675 	spin_unlock_irqrestore(&vfe->output_lock, flags);
676 }
677 
678 /*
679  * vfe_queue_buffer - Add empty buffer
680  * @vid: Video device structure
681  * @buf: Buffer to be enqueued
682  *
683  * Add an empty buffer - depending on the current number of buffers it will be
684  * put in pending buffer queue or directly given to the hardware to be filled.
685  *
686  * Return 0 on success or a negative error code otherwise
687  */
688 static int vfe_queue_buffer(struct camss_video *vid, struct camss_buffer *buf)
689 {
690 	struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
691 	struct vfe_device *vfe = to_vfe(line);
692 	struct vfe_output *output;
693 	unsigned long flags;
694 
695 	output = &line->output;
696 
697 	spin_lock_irqsave(&vfe->output_lock, flags);
698 
699 	vfe_buf_update_wm_on_new(vfe, output, buf, line);
700 
701 	spin_unlock_irqrestore(&vfe->output_lock, flags);
702 
703 	return 0;
704 }
705 
706 #define CALC_WORD(width, M, N) (((width) * (M) + (N) - 1) / (N))
707 
708 int vfe_word_per_line(u32 format, u32 width)
709 {
710 	int val = 0;
711 
712 	switch (format) {
713 	case V4L2_PIX_FMT_NV12:
714 	case V4L2_PIX_FMT_NV21:
715 	case V4L2_PIX_FMT_NV16:
716 	case V4L2_PIX_FMT_NV61:
717 		val = CALC_WORD(width, 1, 8);
718 		break;
719 	case V4L2_PIX_FMT_YUYV:
720 	case V4L2_PIX_FMT_YVYU:
721 	case V4L2_PIX_FMT_UYVY:
722 	case V4L2_PIX_FMT_VYUY:
723 		val = CALC_WORD(width, 2, 8);
724 		break;
725 	}
726 
727 	return val;
728 }
729 
730 const struct vfe_isr_ops vfe_isr_ops_gen1 = {
731 	.reset_ack = vfe_isr_reset_ack,
732 	.halt_ack = vfe_isr_halt_ack,
733 	.reg_update = vfe_isr_reg_update,
734 	.sof = vfe_isr_sof,
735 	.comp_done = vfe_isr_comp_done,
736 	.wm_done = vfe_isr_wm_done,
737 };
738 
739 const struct camss_video_ops vfe_video_ops_gen1 = {
740 	.queue_buffer = vfe_queue_buffer,
741 	.flush_buffers = vfe_flush_buffers,
742 };
743