1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * s3c24xx/s3c64xx SoC series Camera Interface (CAMIF) driver
4  *
5  * Copyright (C) 2012 Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
6  * Copyright (C) 2012 Tomasz Figa <tomasz.figa@gmail.com>
7  *
8  * Based on drivers/media/platform/s5p-fimc,
9  * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
10 */
11 #define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
12 
13 #include <linux/bug.h>
14 #include <linux/clk.h>
15 #include <linux/device.h>
16 #include <linux/errno.h>
17 #include <linux/i2c.h>
18 #include <linux/interrupt.h>
19 #include <linux/io.h>
20 #include <linux/kernel.h>
21 #include <linux/list.h>
22 #include <linux/module.h>
23 #include <linux/platform_device.h>
24 #include <linux/pm_runtime.h>
25 #include <linux/ratelimit.h>
26 #include <linux/slab.h>
27 #include <linux/types.h>
28 #include <linux/videodev2.h>
29 
30 #include <media/media-device.h>
31 #include <media/v4l2-ctrls.h>
32 #include <media/v4l2-event.h>
33 #include <media/v4l2-ioctl.h>
34 #include <media/videobuf2-v4l2.h>
35 #include <media/videobuf2-dma-contig.h>
36 
37 #include "camif-core.h"
38 #include "camif-regs.h"
39 
40 static int debug;
41 module_param(debug, int, 0644);
42 
43 /* Locking: called with vp->camif->slock spinlock held */
44 static void camif_cfg_video_path(struct camif_vp *vp)
45 {
46 	WARN_ON(s3c_camif_get_scaler_config(vp, &vp->scaler));
47 	camif_hw_set_scaler(vp);
48 	camif_hw_set_flip(vp);
49 	camif_hw_set_target_format(vp);
50 	camif_hw_set_output_dma(vp);
51 }
52 
53 static void camif_prepare_dma_offset(struct camif_vp *vp)
54 {
55 	struct camif_frame *f = &vp->out_frame;
56 
57 	f->dma_offset.initial = f->rect.top * f->f_width + f->rect.left;
58 	f->dma_offset.line = f->f_width - (f->rect.left + f->rect.width);
59 
60 	pr_debug("dma_offset: initial: %d, line: %d\n",
61 		 f->dma_offset.initial, f->dma_offset.line);
62 }
63 
64 /* Locking: called with camif->slock spinlock held */
65 static int s3c_camif_hw_init(struct camif_dev *camif, struct camif_vp *vp)
66 {
67 	const struct s3c_camif_variant *variant = camif->variant;
68 
69 	if (camif->sensor.sd == NULL || vp->out_fmt == NULL)
70 		return -EINVAL;
71 
72 	if (variant->ip_revision == S3C244X_CAMIF_IP_REV)
73 		camif_hw_clear_fifo_overflow(vp);
74 	camif_hw_set_camera_bus(camif);
75 	camif_hw_set_source_format(camif);
76 	camif_hw_set_camera_crop(camif);
77 	camif_hw_set_test_pattern(camif, camif->test_pattern);
78 	if (variant->has_img_effect)
79 		camif_hw_set_effect(camif, camif->colorfx,
80 				camif->colorfx_cr, camif->colorfx_cb);
81 	if (variant->ip_revision == S3C6410_CAMIF_IP_REV)
82 		camif_hw_set_input_path(vp);
83 	camif_cfg_video_path(vp);
84 	vp->state &= ~ST_VP_CONFIG;
85 
86 	return 0;
87 }
88 
89 /*
90  * Initialize the video path, only up from the scaler stage. The camera
91  * input interface set up is skipped. This is useful to enable one of the
92  * video paths when the other is already running.
93  * Locking: called with camif->slock spinlock held.
94  */
95 static int s3c_camif_hw_vp_init(struct camif_dev *camif, struct camif_vp *vp)
96 {
97 	unsigned int ip_rev = camif->variant->ip_revision;
98 
99 	if (vp->out_fmt == NULL)
100 		return -EINVAL;
101 
102 	camif_prepare_dma_offset(vp);
103 	if (ip_rev == S3C244X_CAMIF_IP_REV)
104 		camif_hw_clear_fifo_overflow(vp);
105 	camif_cfg_video_path(vp);
106 	vp->state &= ~ST_VP_CONFIG;
107 	return 0;
108 }
109 
110 static int sensor_set_power(struct camif_dev *camif, int on)
111 {
112 	struct cam_sensor *sensor = &camif->sensor;
113 	int err = 0;
114 
115 	if (camif->sensor.power_count == !on)
116 		err = v4l2_subdev_call(sensor->sd, core, s_power, on);
117 	if (err == -ENOIOCTLCMD)
118 		err = 0;
119 	if (!err)
120 		sensor->power_count += on ? 1 : -1;
121 
122 	pr_debug("on: %d, power_count: %d, err: %d\n",
123 		 on, sensor->power_count, err);
124 
125 	return err;
126 }
127 
128 static int sensor_set_streaming(struct camif_dev *camif, int on)
129 {
130 	struct cam_sensor *sensor = &camif->sensor;
131 	int err = 0;
132 
133 	if (camif->sensor.stream_count == !on)
134 		err = v4l2_subdev_call(sensor->sd, video, s_stream, on);
135 	if (!err)
136 		sensor->stream_count += on ? 1 : -1;
137 
138 	pr_debug("on: %d, stream_count: %d, err: %d\n",
139 		 on, sensor->stream_count, err);
140 
141 	return err;
142 }
143 
144 /*
145  * Reinitialize the driver so it is ready to start streaming again.
146  * Return any buffers to vb2, perform CAMIF software reset and
147  * turn off streaming at the data pipeline (sensor) if required.
148  */
149 static int camif_reinitialize(struct camif_vp *vp)
150 {
151 	struct camif_dev *camif = vp->camif;
152 	struct camif_buffer *buf;
153 	unsigned long flags;
154 	bool streaming;
155 
156 	spin_lock_irqsave(&camif->slock, flags);
157 	streaming = vp->state & ST_VP_SENSOR_STREAMING;
158 
159 	vp->state &= ~(ST_VP_PENDING | ST_VP_RUNNING | ST_VP_OFF |
160 		       ST_VP_ABORTING | ST_VP_STREAMING |
161 		       ST_VP_SENSOR_STREAMING | ST_VP_LASTIRQ);
162 
163 	/* Release unused buffers */
164 	while (!list_empty(&vp->pending_buf_q)) {
165 		buf = camif_pending_queue_pop(vp);
166 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
167 	}
168 
169 	while (!list_empty(&vp->active_buf_q)) {
170 		buf = camif_active_queue_pop(vp);
171 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
172 	}
173 
174 	spin_unlock_irqrestore(&camif->slock, flags);
175 
176 	if (!streaming)
177 		return 0;
178 
179 	return sensor_set_streaming(camif, 0);
180 }
181 
182 static bool s3c_vp_active(struct camif_vp *vp)
183 {
184 	struct camif_dev *camif = vp->camif;
185 	unsigned long flags;
186 	bool ret;
187 
188 	spin_lock_irqsave(&camif->slock, flags);
189 	ret = (vp->state & ST_VP_RUNNING) || (vp->state & ST_VP_PENDING);
190 	spin_unlock_irqrestore(&camif->slock, flags);
191 
192 	return ret;
193 }
194 
195 static bool camif_is_streaming(struct camif_dev *camif)
196 {
197 	unsigned long flags;
198 	bool status;
199 
200 	spin_lock_irqsave(&camif->slock, flags);
201 	status = camif->stream_count > 0;
202 	spin_unlock_irqrestore(&camif->slock, flags);
203 
204 	return status;
205 }
206 
207 static int camif_stop_capture(struct camif_vp *vp)
208 {
209 	struct camif_dev *camif = vp->camif;
210 	unsigned long flags;
211 	int ret;
212 
213 	if (!s3c_vp_active(vp))
214 		return 0;
215 
216 	spin_lock_irqsave(&camif->slock, flags);
217 	vp->state &= ~(ST_VP_OFF | ST_VP_LASTIRQ);
218 	vp->state |= ST_VP_ABORTING;
219 	spin_unlock_irqrestore(&camif->slock, flags);
220 
221 	ret = wait_event_timeout(vp->irq_queue,
222 			   !(vp->state & ST_VP_ABORTING),
223 			   msecs_to_jiffies(CAMIF_STOP_TIMEOUT));
224 
225 	spin_lock_irqsave(&camif->slock, flags);
226 
227 	if (ret == 0 && !(vp->state & ST_VP_OFF)) {
228 		/* Timed out, forcibly stop capture */
229 		vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
230 			       ST_VP_LASTIRQ);
231 
232 		camif_hw_disable_capture(vp);
233 		camif_hw_enable_scaler(vp, false);
234 	}
235 
236 	spin_unlock_irqrestore(&camif->slock, flags);
237 
238 	return camif_reinitialize(vp);
239 }
240 
241 static int camif_prepare_addr(struct camif_vp *vp, struct vb2_buffer *vb,
242 			      struct camif_addr *paddr)
243 {
244 	struct camif_frame *frame = &vp->out_frame;
245 	u32 pix_size;
246 
247 	if (vb == NULL || frame == NULL)
248 		return -EINVAL;
249 
250 	pix_size = frame->rect.width * frame->rect.height;
251 
252 	pr_debug("colplanes: %d, pix_size: %u\n",
253 		 vp->out_fmt->colplanes, pix_size);
254 
255 	paddr->y = vb2_dma_contig_plane_dma_addr(vb, 0);
256 
257 	switch (vp->out_fmt->colplanes) {
258 	case 1:
259 		paddr->cb = 0;
260 		paddr->cr = 0;
261 		break;
262 	case 2:
263 		/* decompose Y into Y/Cb */
264 		paddr->cb = (u32)(paddr->y + pix_size);
265 		paddr->cr = 0;
266 		break;
267 	case 3:
268 		paddr->cb = (u32)(paddr->y + pix_size);
269 		/* decompose Y into Y/Cb/Cr */
270 		if (vp->out_fmt->color == IMG_FMT_YCBCR422P)
271 			paddr->cr = (u32)(paddr->cb + (pix_size >> 1));
272 		else /* 420 */
273 			paddr->cr = (u32)(paddr->cb + (pix_size >> 2));
274 
275 		if (vp->out_fmt->color == IMG_FMT_YCRCB420)
276 			swap(paddr->cb, paddr->cr);
277 		break;
278 	default:
279 		return -EINVAL;
280 	}
281 
282 	pr_debug("DMA address: y: %pad  cb: %pad cr: %pad\n",
283 		 &paddr->y, &paddr->cb, &paddr->cr);
284 
285 	return 0;
286 }
287 
288 irqreturn_t s3c_camif_irq_handler(int irq, void *priv)
289 {
290 	struct camif_vp *vp = priv;
291 	struct camif_dev *camif = vp->camif;
292 	unsigned int ip_rev = camif->variant->ip_revision;
293 	unsigned int status;
294 
295 	spin_lock(&camif->slock);
296 
297 	if (ip_rev == S3C6410_CAMIF_IP_REV)
298 		camif_hw_clear_pending_irq(vp);
299 
300 	status = camif_hw_get_status(vp);
301 
302 	if (ip_rev == S3C244X_CAMIF_IP_REV && (status & CISTATUS_OVF_MASK)) {
303 		camif_hw_clear_fifo_overflow(vp);
304 		goto unlock;
305 	}
306 
307 	if (vp->state & ST_VP_ABORTING) {
308 		if (vp->state & ST_VP_OFF) {
309 			/* Last IRQ */
310 			vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
311 				       ST_VP_LASTIRQ);
312 			wake_up(&vp->irq_queue);
313 			goto unlock;
314 		} else if (vp->state & ST_VP_LASTIRQ) {
315 			camif_hw_disable_capture(vp);
316 			camif_hw_enable_scaler(vp, false);
317 			camif_hw_set_lastirq(vp, false);
318 			vp->state |= ST_VP_OFF;
319 		} else {
320 			/* Disable capture, enable last IRQ */
321 			camif_hw_set_lastirq(vp, true);
322 			vp->state |= ST_VP_LASTIRQ;
323 		}
324 	}
325 
326 	if (!list_empty(&vp->pending_buf_q) && (vp->state & ST_VP_RUNNING) &&
327 	    !list_empty(&vp->active_buf_q)) {
328 		unsigned int index;
329 		struct camif_buffer *vbuf;
330 		/*
331 		 * Get previous DMA write buffer index:
332 		 * 0 => DMA buffer 0, 2;
333 		 * 1 => DMA buffer 1, 3.
334 		 */
335 		index = (CISTATUS_FRAMECNT(status) + 2) & 1;
336 		vbuf = camif_active_queue_peek(vp, index);
337 
338 		if (!WARN_ON(vbuf == NULL)) {
339 			/* Dequeue a filled buffer */
340 			vbuf->vb.vb2_buf.timestamp = ktime_get_ns();
341 			vbuf->vb.sequence = vp->frame_sequence++;
342 			vb2_buffer_done(&vbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
343 
344 			/* Set up an empty buffer at the DMA engine */
345 			vbuf = camif_pending_queue_pop(vp);
346 			vbuf->index = index;
347 			camif_hw_set_output_addr(vp, &vbuf->paddr, index);
348 			camif_hw_set_output_addr(vp, &vbuf->paddr, index + 2);
349 
350 			/* Scheduled in H/W, add to the queue */
351 			camif_active_queue_add(vp, vbuf);
352 		}
353 	} else if (!(vp->state & ST_VP_ABORTING) &&
354 		   (vp->state & ST_VP_PENDING))  {
355 		vp->state |= ST_VP_RUNNING;
356 	}
357 
358 	if (vp->state & ST_VP_CONFIG) {
359 		camif_prepare_dma_offset(vp);
360 		camif_hw_set_camera_crop(camif);
361 		camif_hw_set_scaler(vp);
362 		camif_hw_set_flip(vp);
363 		camif_hw_set_test_pattern(camif, camif->test_pattern);
364 		if (camif->variant->has_img_effect)
365 			camif_hw_set_effect(camif, camif->colorfx,
366 				    camif->colorfx_cr, camif->colorfx_cb);
367 		vp->state &= ~ST_VP_CONFIG;
368 	}
369 unlock:
370 	spin_unlock(&camif->slock);
371 	return IRQ_HANDLED;
372 }
373 
374 static int start_streaming(struct vb2_queue *vq, unsigned int count)
375 {
376 	struct camif_vp *vp = vb2_get_drv_priv(vq);
377 	struct camif_dev *camif = vp->camif;
378 	unsigned long flags;
379 	int ret;
380 
381 	/*
382 	 * We assume the codec capture path is always activated
383 	 * first, before the preview path starts streaming.
384 	 * This is required to avoid internal FIFO overflow and
385 	 * a need for CAMIF software reset.
386 	 */
387 	spin_lock_irqsave(&camif->slock, flags);
388 
389 	if (camif->stream_count == 0) {
390 		camif_hw_reset(camif);
391 		ret = s3c_camif_hw_init(camif, vp);
392 	} else {
393 		ret = s3c_camif_hw_vp_init(camif, vp);
394 	}
395 	spin_unlock_irqrestore(&camif->slock, flags);
396 
397 	if (ret < 0) {
398 		camif_reinitialize(vp);
399 		return ret;
400 	}
401 
402 	spin_lock_irqsave(&camif->slock, flags);
403 	vp->frame_sequence = 0;
404 	vp->state |= ST_VP_PENDING;
405 
406 	if (!list_empty(&vp->pending_buf_q) &&
407 	    (!(vp->state & ST_VP_STREAMING) ||
408 	     !(vp->state & ST_VP_SENSOR_STREAMING))) {
409 
410 		camif_hw_enable_scaler(vp, vp->scaler.enable);
411 		camif_hw_enable_capture(vp);
412 		vp->state |= ST_VP_STREAMING;
413 
414 		if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
415 			vp->state |= ST_VP_SENSOR_STREAMING;
416 			spin_unlock_irqrestore(&camif->slock, flags);
417 			ret = sensor_set_streaming(camif, 1);
418 			if (ret)
419 				v4l2_err(&vp->vdev, "Sensor s_stream failed\n");
420 			if (debug)
421 				camif_hw_dump_regs(camif, __func__);
422 
423 			return ret;
424 		}
425 	}
426 
427 	spin_unlock_irqrestore(&camif->slock, flags);
428 	return 0;
429 }
430 
431 static void stop_streaming(struct vb2_queue *vq)
432 {
433 	struct camif_vp *vp = vb2_get_drv_priv(vq);
434 	camif_stop_capture(vp);
435 }
436 
437 static int queue_setup(struct vb2_queue *vq,
438 		       unsigned int *num_buffers, unsigned int *num_planes,
439 		       unsigned int sizes[], struct device *alloc_devs[])
440 {
441 	struct camif_vp *vp = vb2_get_drv_priv(vq);
442 	struct camif_frame *frame = &vp->out_frame;
443 	const struct camif_fmt *fmt = vp->out_fmt;
444 	unsigned int size;
445 
446 	if (fmt == NULL)
447 		return -EINVAL;
448 
449 	size = (frame->f_width * frame->f_height * fmt->depth) / 8;
450 
451 	if (*num_planes)
452 		return sizes[0] < size ? -EINVAL : 0;
453 
454 	*num_planes = 1;
455 	sizes[0] = size;
456 
457 	pr_debug("size: %u\n", sizes[0]);
458 	return 0;
459 }
460 
461 static int buffer_prepare(struct vb2_buffer *vb)
462 {
463 	struct camif_vp *vp = vb2_get_drv_priv(vb->vb2_queue);
464 
465 	if (vp->out_fmt == NULL)
466 		return -EINVAL;
467 
468 	if (vb2_plane_size(vb, 0) < vp->payload) {
469 		v4l2_err(&vp->vdev, "buffer too small: %lu, required: %u\n",
470 			 vb2_plane_size(vb, 0), vp->payload);
471 		return -EINVAL;
472 	}
473 	vb2_set_plane_payload(vb, 0, vp->payload);
474 
475 	return 0;
476 }
477 
478 static void buffer_queue(struct vb2_buffer *vb)
479 {
480 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
481 	struct camif_buffer *buf = container_of(vbuf, struct camif_buffer, vb);
482 	struct camif_vp *vp = vb2_get_drv_priv(vb->vb2_queue);
483 	struct camif_dev *camif = vp->camif;
484 	unsigned long flags;
485 
486 	spin_lock_irqsave(&camif->slock, flags);
487 	WARN_ON(camif_prepare_addr(vp, &buf->vb.vb2_buf, &buf->paddr));
488 
489 	if (!(vp->state & ST_VP_STREAMING) && vp->active_buffers < 2) {
490 		/* Schedule an empty buffer in H/W */
491 		buf->index = vp->buf_index;
492 
493 		camif_hw_set_output_addr(vp, &buf->paddr, buf->index);
494 		camif_hw_set_output_addr(vp, &buf->paddr, buf->index + 2);
495 
496 		camif_active_queue_add(vp, buf);
497 		vp->buf_index = !vp->buf_index;
498 	} else {
499 		camif_pending_queue_add(vp, buf);
500 	}
501 
502 	if (vb2_is_streaming(&vp->vb_queue) && !list_empty(&vp->pending_buf_q)
503 		&& !(vp->state & ST_VP_STREAMING)) {
504 
505 		vp->state |= ST_VP_STREAMING;
506 		camif_hw_enable_scaler(vp, vp->scaler.enable);
507 		camif_hw_enable_capture(vp);
508 		spin_unlock_irqrestore(&camif->slock, flags);
509 
510 		if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
511 			if (sensor_set_streaming(camif, 1) == 0)
512 				vp->state |= ST_VP_SENSOR_STREAMING;
513 			else
514 				v4l2_err(&vp->vdev, "Sensor s_stream failed\n");
515 
516 			if (debug)
517 				camif_hw_dump_regs(camif, __func__);
518 		}
519 		return;
520 	}
521 	spin_unlock_irqrestore(&camif->slock, flags);
522 }
523 
524 static const struct vb2_ops s3c_camif_qops = {
525 	.queue_setup	 = queue_setup,
526 	.buf_prepare	 = buffer_prepare,
527 	.buf_queue	 = buffer_queue,
528 	.wait_prepare	 = vb2_ops_wait_prepare,
529 	.wait_finish	 = vb2_ops_wait_finish,
530 	.start_streaming = start_streaming,
531 	.stop_streaming	 = stop_streaming,
532 };
533 
534 static int s3c_camif_open(struct file *file)
535 {
536 	struct camif_vp *vp = video_drvdata(file);
537 	struct camif_dev *camif = vp->camif;
538 	int ret;
539 
540 	pr_debug("[vp%d] state: %#x,  owner: %p, pid: %d\n", vp->id,
541 		 vp->state, vp->owner, task_pid_nr(current));
542 
543 	if (mutex_lock_interruptible(&camif->lock))
544 		return -ERESTARTSYS;
545 
546 	ret = v4l2_fh_open(file);
547 	if (ret < 0)
548 		goto unlock;
549 
550 	ret = pm_runtime_resume_and_get(camif->dev);
551 	if (ret < 0)
552 		goto err_pm;
553 
554 	ret = sensor_set_power(camif, 1);
555 	if (!ret)
556 		goto unlock;
557 
558 	pm_runtime_put(camif->dev);
559 err_pm:
560 	v4l2_fh_release(file);
561 unlock:
562 	mutex_unlock(&camif->lock);
563 	return ret;
564 }
565 
566 static int s3c_camif_close(struct file *file)
567 {
568 	struct camif_vp *vp = video_drvdata(file);
569 	struct camif_dev *camif = vp->camif;
570 	int ret;
571 
572 	pr_debug("[vp%d] state: %#x, owner: %p, pid: %d\n", vp->id,
573 		 vp->state, vp->owner, task_pid_nr(current));
574 
575 	mutex_lock(&camif->lock);
576 
577 	if (vp->owner == file->private_data) {
578 		camif_stop_capture(vp);
579 		vb2_queue_release(&vp->vb_queue);
580 		vp->owner = NULL;
581 	}
582 
583 	sensor_set_power(camif, 0);
584 
585 	pm_runtime_put(camif->dev);
586 	ret = v4l2_fh_release(file);
587 
588 	mutex_unlock(&camif->lock);
589 	return ret;
590 }
591 
592 static __poll_t s3c_camif_poll(struct file *file,
593 				   struct poll_table_struct *wait)
594 {
595 	struct camif_vp *vp = video_drvdata(file);
596 	struct camif_dev *camif = vp->camif;
597 	__poll_t ret;
598 
599 	mutex_lock(&camif->lock);
600 	if (vp->owner && vp->owner != file->private_data)
601 		ret = EPOLLERR;
602 	else
603 		ret = vb2_poll(&vp->vb_queue, file, wait);
604 
605 	mutex_unlock(&camif->lock);
606 	return ret;
607 }
608 
609 static int s3c_camif_mmap(struct file *file, struct vm_area_struct *vma)
610 {
611 	struct camif_vp *vp = video_drvdata(file);
612 	int ret;
613 
614 	if (vp->owner && vp->owner != file->private_data)
615 		ret = -EBUSY;
616 	else
617 		ret = vb2_mmap(&vp->vb_queue, vma);
618 
619 	return ret;
620 }
621 
622 static const struct v4l2_file_operations s3c_camif_fops = {
623 	.owner		= THIS_MODULE,
624 	.open		= s3c_camif_open,
625 	.release	= s3c_camif_close,
626 	.poll		= s3c_camif_poll,
627 	.unlocked_ioctl	= video_ioctl2,
628 	.mmap		= s3c_camif_mmap,
629 };
630 
631 /*
632  * Video node IOCTLs
633  */
634 
635 static int s3c_camif_vidioc_querycap(struct file *file, void *priv,
636 				     struct v4l2_capability *cap)
637 {
638 	struct camif_vp *vp = video_drvdata(file);
639 
640 	strscpy(cap->driver, S3C_CAMIF_DRIVER_NAME, sizeof(cap->driver));
641 	strscpy(cap->card, S3C_CAMIF_DRIVER_NAME, sizeof(cap->card));
642 	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s.%d",
643 		 dev_name(vp->camif->dev), vp->id);
644 	return 0;
645 }
646 
647 static int s3c_camif_vidioc_enum_input(struct file *file, void *priv,
648 				       struct v4l2_input *input)
649 {
650 	struct camif_vp *vp = video_drvdata(file);
651 	struct v4l2_subdev *sensor = vp->camif->sensor.sd;
652 
653 	if (input->index || sensor == NULL)
654 		return -EINVAL;
655 
656 	input->type = V4L2_INPUT_TYPE_CAMERA;
657 	strscpy(input->name, sensor->name, sizeof(input->name));
658 	return 0;
659 }
660 
661 static int s3c_camif_vidioc_s_input(struct file *file, void *priv,
662 				    unsigned int i)
663 {
664 	return i == 0 ? 0 : -EINVAL;
665 }
666 
667 static int s3c_camif_vidioc_g_input(struct file *file, void *priv,
668 				    unsigned int *i)
669 {
670 	*i = 0;
671 	return 0;
672 }
673 
674 static int s3c_camif_vidioc_enum_fmt(struct file *file, void *priv,
675 				     struct v4l2_fmtdesc *f)
676 {
677 	struct camif_vp *vp = video_drvdata(file);
678 	const struct camif_fmt *fmt;
679 
680 	fmt = s3c_camif_find_format(vp, NULL, f->index);
681 	if (!fmt)
682 		return -EINVAL;
683 
684 	f->pixelformat = fmt->fourcc;
685 	return 0;
686 }
687 
688 static int s3c_camif_vidioc_g_fmt(struct file *file, void *priv,
689 				  struct v4l2_format *f)
690 {
691 	struct camif_vp *vp = video_drvdata(file);
692 	struct v4l2_pix_format *pix = &f->fmt.pix;
693 	struct camif_frame *frame = &vp->out_frame;
694 	const struct camif_fmt *fmt = vp->out_fmt;
695 
696 	pix->bytesperline = frame->f_width * fmt->ybpp;
697 	pix->sizeimage = vp->payload;
698 
699 	pix->pixelformat = fmt->fourcc;
700 	pix->width = frame->f_width;
701 	pix->height = frame->f_height;
702 	pix->field = V4L2_FIELD_NONE;
703 	pix->colorspace = V4L2_COLORSPACE_JPEG;
704 
705 	return 0;
706 }
707 
708 static int __camif_video_try_format(struct camif_vp *vp,
709 				    struct v4l2_pix_format *pix,
710 				    const struct camif_fmt **ffmt)
711 {
712 	struct camif_dev *camif = vp->camif;
713 	struct v4l2_rect *crop = &camif->camif_crop;
714 	unsigned int wmin, hmin, sc_hrmax, sc_vrmax;
715 	const struct vp_pix_limits *pix_lim;
716 	const struct camif_fmt *fmt;
717 
718 	fmt = s3c_camif_find_format(vp, &pix->pixelformat, 0);
719 
720 	if (WARN_ON(fmt == NULL))
721 		return -EINVAL;
722 
723 	if (ffmt)
724 		*ffmt = fmt;
725 
726 	pix_lim = &camif->variant->vp_pix_limits[vp->id];
727 
728 	pr_debug("fmt: %ux%u, crop: %ux%u, bytesperline: %u\n",
729 		 pix->width, pix->height, crop->width, crop->height,
730 		 pix->bytesperline);
731 	/*
732 	 * Calculate minimum width and height according to the configured
733 	 * camera input interface crop rectangle and the resizer's capabilities.
734 	 */
735 	sc_hrmax = min(SCALER_MAX_RATIO, 1 << (ffs(crop->width) - 3));
736 	sc_vrmax = min(SCALER_MAX_RATIO, 1 << (ffs(crop->height) - 1));
737 
738 	wmin = max_t(u32, pix_lim->min_out_width, crop->width / sc_hrmax);
739 	wmin = round_up(wmin, pix_lim->out_width_align);
740 	hmin = max_t(u32, 8, crop->height / sc_vrmax);
741 	hmin = round_up(hmin, 8);
742 
743 	v4l_bound_align_image(&pix->width, wmin, pix_lim->max_sc_out_width,
744 			      ffs(pix_lim->out_width_align) - 1,
745 			      &pix->height, hmin, pix_lim->max_height, 0, 0);
746 
747 	pix->bytesperline = pix->width * fmt->ybpp;
748 	pix->sizeimage = (pix->width * pix->height * fmt->depth) / 8;
749 	pix->pixelformat = fmt->fourcc;
750 	pix->colorspace = V4L2_COLORSPACE_JPEG;
751 	pix->field = V4L2_FIELD_NONE;
752 
753 	pr_debug("%ux%u, wmin: %d, hmin: %d, sc_hrmax: %d, sc_vrmax: %d\n",
754 		 pix->width, pix->height, wmin, hmin, sc_hrmax, sc_vrmax);
755 
756 	return 0;
757 }
758 
759 static int s3c_camif_vidioc_try_fmt(struct file *file, void *priv,
760 				    struct v4l2_format *f)
761 {
762 	struct camif_vp *vp = video_drvdata(file);
763 	return __camif_video_try_format(vp, &f->fmt.pix, NULL);
764 }
765 
766 static int s3c_camif_vidioc_s_fmt(struct file *file, void *priv,
767 				  struct v4l2_format *f)
768 {
769 	struct v4l2_pix_format *pix = &f->fmt.pix;
770 	struct camif_vp *vp = video_drvdata(file);
771 	struct camif_frame *out_frame = &vp->out_frame;
772 	const struct camif_fmt *fmt = NULL;
773 	int ret;
774 
775 	pr_debug("[vp%d]\n", vp->id);
776 
777 	if (vb2_is_busy(&vp->vb_queue))
778 		return -EBUSY;
779 
780 	ret = __camif_video_try_format(vp, &f->fmt.pix, &fmt);
781 	if (ret < 0)
782 		return ret;
783 
784 	vp->out_fmt = fmt;
785 	vp->payload = pix->sizeimage;
786 	out_frame->f_width = pix->width;
787 	out_frame->f_height = pix->height;
788 
789 	/* Reset composition rectangle */
790 	out_frame->rect.width = pix->width;
791 	out_frame->rect.height = pix->height;
792 	out_frame->rect.left = 0;
793 	out_frame->rect.top = 0;
794 
795 	if (vp->owner == NULL)
796 		vp->owner = priv;
797 
798 	pr_debug("%ux%u. payload: %u. fmt: 0x%08x. %d %d. sizeimage: %d. bpl: %d\n",
799 		 out_frame->f_width, out_frame->f_height, vp->payload,
800 		 fmt->fourcc, pix->width * pix->height * fmt->depth,
801 		 fmt->depth, pix->sizeimage, pix->bytesperline);
802 
803 	return 0;
804 }
805 
806 /* Only check pixel formats at the sensor and the camif subdev pads */
807 static int camif_pipeline_validate(struct camif_dev *camif)
808 {
809 	struct v4l2_subdev_format src_fmt = {
810 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
811 	};
812 	struct media_pad *pad;
813 	int ret;
814 
815 	/* Retrieve format at the sensor subdev source pad */
816 	pad = media_pad_remote_pad_first(&camif->pads[0]);
817 	if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
818 		return -EPIPE;
819 
820 	src_fmt.pad = pad->index;
821 	ret = v4l2_subdev_call(camif->sensor.sd, pad, get_fmt, NULL, &src_fmt);
822 	if (ret < 0 && ret != -ENOIOCTLCMD)
823 		return -EPIPE;
824 
825 	if (src_fmt.format.width != camif->mbus_fmt.width ||
826 	    src_fmt.format.height != camif->mbus_fmt.height ||
827 	    src_fmt.format.code != camif->mbus_fmt.code)
828 		return -EPIPE;
829 
830 	return 0;
831 }
832 
833 static int s3c_camif_streamon(struct file *file, void *priv,
834 			      enum v4l2_buf_type type)
835 {
836 	struct camif_vp *vp = video_drvdata(file);
837 	struct camif_dev *camif = vp->camif;
838 	struct media_entity *sensor = &camif->sensor.sd->entity;
839 	int ret;
840 
841 	pr_debug("[vp%d]\n", vp->id);
842 
843 	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
844 		return -EINVAL;
845 
846 	if (vp->owner && vp->owner != priv)
847 		return -EBUSY;
848 
849 	if (s3c_vp_active(vp))
850 		return 0;
851 
852 	ret = media_pipeline_start(sensor->pads, camif->m_pipeline);
853 	if (ret < 0)
854 		return ret;
855 
856 	ret = camif_pipeline_validate(camif);
857 	if (ret < 0) {
858 		media_pipeline_stop(sensor->pads);
859 		return ret;
860 	}
861 
862 	return vb2_streamon(&vp->vb_queue, type);
863 }
864 
865 static int s3c_camif_streamoff(struct file *file, void *priv,
866 			       enum v4l2_buf_type type)
867 {
868 	struct camif_vp *vp = video_drvdata(file);
869 	struct camif_dev *camif = vp->camif;
870 	int ret;
871 
872 	pr_debug("[vp%d]\n", vp->id);
873 
874 	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
875 		return -EINVAL;
876 
877 	if (vp->owner && vp->owner != priv)
878 		return -EBUSY;
879 
880 	ret = vb2_streamoff(&vp->vb_queue, type);
881 	if (ret == 0)
882 		media_pipeline_stop(camif->sensor.sd->entity.pads);
883 	return ret;
884 }
885 
886 static int s3c_camif_reqbufs(struct file *file, void *priv,
887 			     struct v4l2_requestbuffers *rb)
888 {
889 	struct camif_vp *vp = video_drvdata(file);
890 	int ret;
891 
892 	pr_debug("[vp%d] rb count: %d, owner: %p, priv: %p\n",
893 		 vp->id, rb->count, vp->owner, priv);
894 
895 	if (vp->owner && vp->owner != priv)
896 		return -EBUSY;
897 
898 	if (rb->count)
899 		rb->count = max_t(u32, CAMIF_REQ_BUFS_MIN, rb->count);
900 	else
901 		vp->owner = NULL;
902 
903 	ret = vb2_reqbufs(&vp->vb_queue, rb);
904 	if (ret < 0)
905 		return ret;
906 
907 	if (rb->count && rb->count < CAMIF_REQ_BUFS_MIN) {
908 		rb->count = 0;
909 		vb2_reqbufs(&vp->vb_queue, rb);
910 		ret = -ENOMEM;
911 	}
912 
913 	vp->reqbufs_count = rb->count;
914 	if (vp->owner == NULL && rb->count > 0)
915 		vp->owner = priv;
916 
917 	return ret;
918 }
919 
920 static int s3c_camif_querybuf(struct file *file, void *priv,
921 			      struct v4l2_buffer *buf)
922 {
923 	struct camif_vp *vp = video_drvdata(file);
924 	return vb2_querybuf(&vp->vb_queue, buf);
925 }
926 
927 static int s3c_camif_qbuf(struct file *file, void *priv,
928 			  struct v4l2_buffer *buf)
929 {
930 	struct camif_vp *vp = video_drvdata(file);
931 
932 	pr_debug("[vp%d]\n", vp->id);
933 
934 	if (vp->owner && vp->owner != priv)
935 		return -EBUSY;
936 
937 	return vb2_qbuf(&vp->vb_queue, vp->vdev.v4l2_dev->mdev, buf);
938 }
939 
940 static int s3c_camif_dqbuf(struct file *file, void *priv,
941 			   struct v4l2_buffer *buf)
942 {
943 	struct camif_vp *vp = video_drvdata(file);
944 
945 	pr_debug("[vp%d] sequence: %d\n", vp->id, vp->frame_sequence);
946 
947 	if (vp->owner && vp->owner != priv)
948 		return -EBUSY;
949 
950 	return vb2_dqbuf(&vp->vb_queue, buf, file->f_flags & O_NONBLOCK);
951 }
952 
953 static int s3c_camif_create_bufs(struct file *file, void *priv,
954 				 struct v4l2_create_buffers *create)
955 {
956 	struct camif_vp *vp = video_drvdata(file);
957 	int ret;
958 
959 	if (vp->owner && vp->owner != priv)
960 		return -EBUSY;
961 
962 	create->count = max_t(u32, 1, create->count);
963 	ret = vb2_create_bufs(&vp->vb_queue, create);
964 
965 	if (!ret && vp->owner == NULL)
966 		vp->owner = priv;
967 
968 	return ret;
969 }
970 
971 static int s3c_camif_prepare_buf(struct file *file, void *priv,
972 				 struct v4l2_buffer *b)
973 {
974 	struct camif_vp *vp = video_drvdata(file);
975 	return vb2_prepare_buf(&vp->vb_queue, vp->vdev.v4l2_dev->mdev, b);
976 }
977 
978 static int s3c_camif_g_selection(struct file *file, void *priv,
979 				 struct v4l2_selection *sel)
980 {
981 	struct camif_vp *vp = video_drvdata(file);
982 
983 	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
984 		return -EINVAL;
985 
986 	switch (sel->target) {
987 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
988 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
989 		sel->r.left = 0;
990 		sel->r.top = 0;
991 		sel->r.width = vp->out_frame.f_width;
992 		sel->r.height = vp->out_frame.f_height;
993 		return 0;
994 
995 	case V4L2_SEL_TGT_COMPOSE:
996 		sel->r = vp->out_frame.rect;
997 		return 0;
998 	}
999 
1000 	return -EINVAL;
1001 }
1002 
1003 static void __camif_try_compose(struct camif_dev *camif, struct camif_vp *vp,
1004 				struct v4l2_rect *r)
1005 {
1006 	/* s3c244x doesn't support composition */
1007 	if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV) {
1008 		*r = vp->out_frame.rect;
1009 		return;
1010 	}
1011 
1012 	/* TODO: s3c64xx */
1013 }
1014 
1015 static int s3c_camif_s_selection(struct file *file, void *priv,
1016 				 struct v4l2_selection *sel)
1017 {
1018 	struct camif_vp *vp = video_drvdata(file);
1019 	struct camif_dev *camif = vp->camif;
1020 	struct v4l2_rect rect = sel->r;
1021 	unsigned long flags;
1022 
1023 	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
1024 	    sel->target != V4L2_SEL_TGT_COMPOSE)
1025 		return -EINVAL;
1026 
1027 	__camif_try_compose(camif, vp, &rect);
1028 
1029 	sel->r = rect;
1030 	spin_lock_irqsave(&camif->slock, flags);
1031 	vp->out_frame.rect = rect;
1032 	vp->state |= ST_VP_CONFIG;
1033 	spin_unlock_irqrestore(&camif->slock, flags);
1034 
1035 	pr_debug("type: %#x, target: %#x, flags: %#x, (%d,%d)/%dx%d\n",
1036 		sel->type, sel->target, sel->flags,
1037 		sel->r.left, sel->r.top, sel->r.width, sel->r.height);
1038 
1039 	return 0;
1040 }
1041 
1042 static const struct v4l2_ioctl_ops s3c_camif_ioctl_ops = {
1043 	.vidioc_querycap	  = s3c_camif_vidioc_querycap,
1044 	.vidioc_enum_input	  = s3c_camif_vidioc_enum_input,
1045 	.vidioc_g_input		  = s3c_camif_vidioc_g_input,
1046 	.vidioc_s_input		  = s3c_camif_vidioc_s_input,
1047 	.vidioc_enum_fmt_vid_cap  = s3c_camif_vidioc_enum_fmt,
1048 	.vidioc_try_fmt_vid_cap	  = s3c_camif_vidioc_try_fmt,
1049 	.vidioc_s_fmt_vid_cap	  = s3c_camif_vidioc_s_fmt,
1050 	.vidioc_g_fmt_vid_cap	  = s3c_camif_vidioc_g_fmt,
1051 	.vidioc_g_selection	  = s3c_camif_g_selection,
1052 	.vidioc_s_selection	  = s3c_camif_s_selection,
1053 	.vidioc_reqbufs		  = s3c_camif_reqbufs,
1054 	.vidioc_querybuf	  = s3c_camif_querybuf,
1055 	.vidioc_prepare_buf	  = s3c_camif_prepare_buf,
1056 	.vidioc_create_bufs	  = s3c_camif_create_bufs,
1057 	.vidioc_qbuf		  = s3c_camif_qbuf,
1058 	.vidioc_dqbuf		  = s3c_camif_dqbuf,
1059 	.vidioc_streamon	  = s3c_camif_streamon,
1060 	.vidioc_streamoff	  = s3c_camif_streamoff,
1061 	.vidioc_subscribe_event	  = v4l2_ctrl_subscribe_event,
1062 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1063 	.vidioc_log_status	  = v4l2_ctrl_log_status,
1064 };
1065 
1066 /*
1067  * Video node controls
1068  */
1069 static int s3c_camif_video_s_ctrl(struct v4l2_ctrl *ctrl)
1070 {
1071 	struct camif_vp *vp = ctrl->priv;
1072 	struct camif_dev *camif = vp->camif;
1073 	unsigned long flags;
1074 
1075 	pr_debug("[vp%d] ctrl: %s, value: %d\n", vp->id,
1076 		 ctrl->name, ctrl->val);
1077 
1078 	spin_lock_irqsave(&camif->slock, flags);
1079 
1080 	switch (ctrl->id) {
1081 	case V4L2_CID_HFLIP:
1082 		vp->hflip = ctrl->val;
1083 		break;
1084 
1085 	case V4L2_CID_VFLIP:
1086 		vp->vflip = ctrl->val;
1087 		break;
1088 	}
1089 
1090 	vp->state |= ST_VP_CONFIG;
1091 	spin_unlock_irqrestore(&camif->slock, flags);
1092 	return 0;
1093 }
1094 
1095 /* Codec and preview video node control ops */
1096 static const struct v4l2_ctrl_ops s3c_camif_video_ctrl_ops = {
1097 	.s_ctrl = s3c_camif_video_s_ctrl,
1098 };
1099 
1100 int s3c_camif_register_video_node(struct camif_dev *camif, int idx)
1101 {
1102 	struct camif_vp *vp = &camif->vp[idx];
1103 	struct vb2_queue *q = &vp->vb_queue;
1104 	struct video_device *vfd = &vp->vdev;
1105 	struct v4l2_ctrl *ctrl;
1106 	int ret;
1107 
1108 	memset(vfd, 0, sizeof(*vfd));
1109 	snprintf(vfd->name, sizeof(vfd->name), "camif-%s",
1110 		 vp->id == 0 ? "codec" : "preview");
1111 
1112 	vfd->fops = &s3c_camif_fops;
1113 	vfd->ioctl_ops = &s3c_camif_ioctl_ops;
1114 	vfd->v4l2_dev = &camif->v4l2_dev;
1115 	vfd->minor = -1;
1116 	vfd->release = video_device_release_empty;
1117 	vfd->lock = &camif->lock;
1118 	vp->reqbufs_count = 0;
1119 
1120 	INIT_LIST_HEAD(&vp->pending_buf_q);
1121 	INIT_LIST_HEAD(&vp->active_buf_q);
1122 
1123 	memset(q, 0, sizeof(*q));
1124 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1125 	q->io_modes = VB2_MMAP | VB2_USERPTR;
1126 	q->ops = &s3c_camif_qops;
1127 	q->mem_ops = &vb2_dma_contig_memops;
1128 	q->buf_struct_size = sizeof(struct camif_buffer);
1129 	q->drv_priv = vp;
1130 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1131 	q->lock = &vp->camif->lock;
1132 	q->dev = camif->v4l2_dev.dev;
1133 
1134 	ret = vb2_queue_init(q);
1135 	if (ret)
1136 		return ret;
1137 
1138 	vp->pad.flags = MEDIA_PAD_FL_SINK;
1139 	ret = media_entity_pads_init(&vfd->entity, 1, &vp->pad);
1140 	if (ret)
1141 		return ret;
1142 
1143 	video_set_drvdata(vfd, vp);
1144 
1145 	v4l2_ctrl_handler_init(&vp->ctrl_handler, 1);
1146 	ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
1147 				 V4L2_CID_HFLIP, 0, 1, 1, 0);
1148 	if (ctrl)
1149 		ctrl->priv = vp;
1150 	ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
1151 				 V4L2_CID_VFLIP, 0, 1, 1, 0);
1152 	if (ctrl)
1153 		ctrl->priv = vp;
1154 
1155 	ret = vp->ctrl_handler.error;
1156 	if (ret < 0)
1157 		goto err_me_cleanup;
1158 
1159 	vfd->ctrl_handler = &vp->ctrl_handler;
1160 	vfd->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
1161 
1162 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, -1);
1163 	if (ret)
1164 		goto err_ctrlh_free;
1165 
1166 	v4l2_info(&camif->v4l2_dev, "registered %s as /dev/%s\n",
1167 		  vfd->name, video_device_node_name(vfd));
1168 	return 0;
1169 
1170 err_ctrlh_free:
1171 	v4l2_ctrl_handler_free(&vp->ctrl_handler);
1172 err_me_cleanup:
1173 	media_entity_cleanup(&vfd->entity);
1174 	return ret;
1175 }
1176 
1177 void s3c_camif_unregister_video_node(struct camif_dev *camif, int idx)
1178 {
1179 	struct video_device *vfd = &camif->vp[idx].vdev;
1180 
1181 	if (video_is_registered(vfd)) {
1182 		video_unregister_device(vfd);
1183 		media_entity_cleanup(&vfd->entity);
1184 		v4l2_ctrl_handler_free(vfd->ctrl_handler);
1185 	}
1186 }
1187 
1188 /* Media bus pixel formats supported at the camif input */
1189 static const u32 camif_mbus_formats[] = {
1190 	MEDIA_BUS_FMT_YUYV8_2X8,
1191 	MEDIA_BUS_FMT_YVYU8_2X8,
1192 	MEDIA_BUS_FMT_UYVY8_2X8,
1193 	MEDIA_BUS_FMT_VYUY8_2X8,
1194 };
1195 
1196 /*
1197  *  Camera input interface subdev operations
1198  */
1199 
1200 static int s3c_camif_subdev_enum_mbus_code(struct v4l2_subdev *sd,
1201 					struct v4l2_subdev_state *sd_state,
1202 					struct v4l2_subdev_mbus_code_enum *code)
1203 {
1204 	if (code->index >= ARRAY_SIZE(camif_mbus_formats))
1205 		return -EINVAL;
1206 
1207 	code->code = camif_mbus_formats[code->index];
1208 	return 0;
1209 }
1210 
1211 static int s3c_camif_subdev_get_fmt(struct v4l2_subdev *sd,
1212 				    struct v4l2_subdev_state *sd_state,
1213 				    struct v4l2_subdev_format *fmt)
1214 {
1215 	struct camif_dev *camif = v4l2_get_subdevdata(sd);
1216 	struct v4l2_mbus_framefmt *mf = &fmt->format;
1217 
1218 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1219 		mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
1220 		fmt->format = *mf;
1221 		return 0;
1222 	}
1223 
1224 	mutex_lock(&camif->lock);
1225 
1226 	switch (fmt->pad) {
1227 	case CAMIF_SD_PAD_SINK:
1228 		/* full camera input pixel size */
1229 		*mf = camif->mbus_fmt;
1230 		break;
1231 
1232 	case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
1233 		/* crop rectangle at camera interface input */
1234 		mf->width = camif->camif_crop.width;
1235 		mf->height = camif->camif_crop.height;
1236 		mf->code = camif->mbus_fmt.code;
1237 		break;
1238 	}
1239 
1240 	mutex_unlock(&camif->lock);
1241 	mf->field = V4L2_FIELD_NONE;
1242 	mf->colorspace = V4L2_COLORSPACE_JPEG;
1243 	return 0;
1244 }
1245 
1246 static void __camif_subdev_try_format(struct camif_dev *camif,
1247 				struct v4l2_mbus_framefmt *mf, int pad)
1248 {
1249 	const struct s3c_camif_variant *variant = camif->variant;
1250 	const struct vp_pix_limits *pix_lim;
1251 	unsigned int i;
1252 
1253 	/* FIXME: constraints against codec or preview path ? */
1254 	pix_lim = &variant->vp_pix_limits[VP_CODEC];
1255 
1256 	for (i = 0; i < ARRAY_SIZE(camif_mbus_formats); i++)
1257 		if (camif_mbus_formats[i] == mf->code)
1258 			break;
1259 
1260 	if (i == ARRAY_SIZE(camif_mbus_formats))
1261 		mf->code = camif_mbus_formats[0];
1262 
1263 	if (pad == CAMIF_SD_PAD_SINK) {
1264 		v4l_bound_align_image(&mf->width, 8, CAMIF_MAX_PIX_WIDTH,
1265 				      ffs(pix_lim->out_width_align) - 1,
1266 				      &mf->height, 8, CAMIF_MAX_PIX_HEIGHT, 0,
1267 				      0);
1268 	} else {
1269 		struct v4l2_rect *crop = &camif->camif_crop;
1270 		v4l_bound_align_image(&mf->width, 8, crop->width,
1271 				      ffs(pix_lim->out_width_align) - 1,
1272 				      &mf->height, 8, crop->height,
1273 				      0, 0);
1274 	}
1275 
1276 	v4l2_dbg(1, debug, &camif->subdev, "%ux%u\n", mf->width, mf->height);
1277 }
1278 
1279 static int s3c_camif_subdev_set_fmt(struct v4l2_subdev *sd,
1280 				    struct v4l2_subdev_state *sd_state,
1281 				    struct v4l2_subdev_format *fmt)
1282 {
1283 	struct camif_dev *camif = v4l2_get_subdevdata(sd);
1284 	struct v4l2_mbus_framefmt *mf = &fmt->format;
1285 	struct v4l2_rect *crop = &camif->camif_crop;
1286 	int i;
1287 
1288 	v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %ux%u\n",
1289 		 fmt->pad, mf->code, mf->width, mf->height);
1290 
1291 	mf->field = V4L2_FIELD_NONE;
1292 	mf->colorspace = V4L2_COLORSPACE_JPEG;
1293 	mutex_lock(&camif->lock);
1294 
1295 	/*
1296 	 * No pixel format change at the camera input is allowed
1297 	 * while streaming.
1298 	 */
1299 	if (vb2_is_busy(&camif->vp[VP_CODEC].vb_queue) ||
1300 	    vb2_is_busy(&camif->vp[VP_PREVIEW].vb_queue)) {
1301 		mutex_unlock(&camif->lock);
1302 		return -EBUSY;
1303 	}
1304 
1305 	__camif_subdev_try_format(camif, mf, fmt->pad);
1306 
1307 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1308 		mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
1309 		*mf = fmt->format;
1310 		mutex_unlock(&camif->lock);
1311 		return 0;
1312 	}
1313 
1314 	switch (fmt->pad) {
1315 	case CAMIF_SD_PAD_SINK:
1316 		camif->mbus_fmt = *mf;
1317 		/* Reset sink crop rectangle. */
1318 		crop->width = mf->width;
1319 		crop->height = mf->height;
1320 		crop->left = 0;
1321 		crop->top = 0;
1322 		/*
1323 		 * Reset source format (the camif's crop rectangle)
1324 		 * and the video output resolution.
1325 		 */
1326 		for (i = 0; i < CAMIF_VP_NUM; i++) {
1327 			struct camif_frame *frame = &camif->vp[i].out_frame;
1328 			frame->rect = *crop;
1329 			frame->f_width = mf->width;
1330 			frame->f_height = mf->height;
1331 		}
1332 		break;
1333 
1334 	case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
1335 		/* Pixel format can be only changed on the sink pad. */
1336 		mf->code = camif->mbus_fmt.code;
1337 		mf->width = crop->width;
1338 		mf->height = crop->height;
1339 		break;
1340 	}
1341 
1342 	mutex_unlock(&camif->lock);
1343 	return 0;
1344 }
1345 
1346 static int s3c_camif_subdev_get_selection(struct v4l2_subdev *sd,
1347 					  struct v4l2_subdev_state *sd_state,
1348 					  struct v4l2_subdev_selection *sel)
1349 {
1350 	struct camif_dev *camif = v4l2_get_subdevdata(sd);
1351 	struct v4l2_rect *crop = &camif->camif_crop;
1352 	struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
1353 
1354 	if ((sel->target != V4L2_SEL_TGT_CROP &&
1355 	    sel->target != V4L2_SEL_TGT_CROP_BOUNDS) ||
1356 	    sel->pad != CAMIF_SD_PAD_SINK)
1357 		return -EINVAL;
1358 
1359 	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1360 		sel->r = *v4l2_subdev_get_try_crop(sd, sd_state, sel->pad);
1361 		return 0;
1362 	}
1363 
1364 	mutex_lock(&camif->lock);
1365 
1366 	if (sel->target == V4L2_SEL_TGT_CROP) {
1367 		sel->r = *crop;
1368 	} else { /* crop bounds */
1369 		sel->r.width = mf->width;
1370 		sel->r.height = mf->height;
1371 		sel->r.left = 0;
1372 		sel->r.top = 0;
1373 	}
1374 
1375 	mutex_unlock(&camif->lock);
1376 
1377 	v4l2_dbg(1, debug, sd, "%s: crop: (%d,%d) %dx%d, size: %ux%u\n",
1378 		 __func__, crop->left, crop->top, crop->width,
1379 		 crop->height, mf->width, mf->height);
1380 
1381 	return 0;
1382 }
1383 
1384 static void __camif_try_crop(struct camif_dev *camif, struct v4l2_rect *r)
1385 {
1386 	struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
1387 	const struct camif_pix_limits *pix_lim = &camif->variant->pix_limits;
1388 	unsigned int left = 2 * r->left;
1389 	unsigned int top = 2 * r->top;
1390 
1391 	/*
1392 	 * Following constraints must be met:
1393 	 *  - r->width + 2 * r->left = mf->width;
1394 	 *  - r->height + 2 * r->top = mf->height;
1395 	 *  - crop rectangle size and position must be aligned
1396 	 *    to 8 or 2 pixels, depending on SoC version.
1397 	 */
1398 	v4l_bound_align_image(&r->width, 0, mf->width,
1399 			      ffs(pix_lim->win_hor_offset_align) - 1,
1400 			      &r->height, 0, mf->height, 1, 0);
1401 
1402 	v4l_bound_align_image(&left, 0, mf->width - r->width,
1403 			      ffs(pix_lim->win_hor_offset_align),
1404 			      &top, 0, mf->height - r->height, 2, 0);
1405 
1406 	r->left = left / 2;
1407 	r->top = top / 2;
1408 	r->width = mf->width - left;
1409 	r->height = mf->height - top;
1410 	/*
1411 	 * Make sure we either downscale or upscale both the pixel
1412 	 * width and height. Just return current crop rectangle if
1413 	 * this scaler constraint is not met.
1414 	 */
1415 	if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV &&
1416 	    camif_is_streaming(camif)) {
1417 		unsigned int i;
1418 
1419 		for (i = 0; i < CAMIF_VP_NUM; i++) {
1420 			struct v4l2_rect *or = &camif->vp[i].out_frame.rect;
1421 			if ((or->width > r->width) == (or->height > r->height))
1422 				continue;
1423 			*r = camif->camif_crop;
1424 			pr_debug("Width/height scaling direction limitation\n");
1425 			break;
1426 		}
1427 	}
1428 
1429 	v4l2_dbg(1, debug, &camif->v4l2_dev, "crop: (%d,%d)/%dx%d, fmt: %ux%u\n",
1430 		 r->left, r->top, r->width, r->height, mf->width, mf->height);
1431 }
1432 
1433 static int s3c_camif_subdev_set_selection(struct v4l2_subdev *sd,
1434 					  struct v4l2_subdev_state *sd_state,
1435 					  struct v4l2_subdev_selection *sel)
1436 {
1437 	struct camif_dev *camif = v4l2_get_subdevdata(sd);
1438 	struct v4l2_rect *crop = &camif->camif_crop;
1439 	struct camif_scaler scaler;
1440 
1441 	if (sel->target != V4L2_SEL_TGT_CROP || sel->pad != CAMIF_SD_PAD_SINK)
1442 		return -EINVAL;
1443 
1444 	mutex_lock(&camif->lock);
1445 	__camif_try_crop(camif, &sel->r);
1446 
1447 	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1448 		*v4l2_subdev_get_try_crop(sd, sd_state, sel->pad) = sel->r;
1449 	} else {
1450 		unsigned long flags;
1451 		unsigned int i;
1452 
1453 		spin_lock_irqsave(&camif->slock, flags);
1454 		*crop = sel->r;
1455 
1456 		for (i = 0; i < CAMIF_VP_NUM; i++) {
1457 			struct camif_vp *vp = &camif->vp[i];
1458 			scaler = vp->scaler;
1459 			if (s3c_camif_get_scaler_config(vp, &scaler))
1460 				continue;
1461 			vp->scaler = scaler;
1462 			vp->state |= ST_VP_CONFIG;
1463 		}
1464 
1465 		spin_unlock_irqrestore(&camif->slock, flags);
1466 	}
1467 	mutex_unlock(&camif->lock);
1468 
1469 	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %u, f_h: %u\n",
1470 		 __func__, crop->left, crop->top, crop->width, crop->height,
1471 		 camif->mbus_fmt.width, camif->mbus_fmt.height);
1472 
1473 	return 0;
1474 }
1475 
1476 static const struct v4l2_subdev_pad_ops s3c_camif_subdev_pad_ops = {
1477 	.enum_mbus_code = s3c_camif_subdev_enum_mbus_code,
1478 	.get_selection = s3c_camif_subdev_get_selection,
1479 	.set_selection = s3c_camif_subdev_set_selection,
1480 	.get_fmt = s3c_camif_subdev_get_fmt,
1481 	.set_fmt = s3c_camif_subdev_set_fmt,
1482 };
1483 
1484 static const struct v4l2_subdev_ops s3c_camif_subdev_ops = {
1485 	.pad = &s3c_camif_subdev_pad_ops,
1486 };
1487 
1488 static int s3c_camif_subdev_s_ctrl(struct v4l2_ctrl *ctrl)
1489 {
1490 	struct camif_dev *camif = container_of(ctrl->handler, struct camif_dev,
1491 					       ctrl_handler);
1492 	unsigned long flags;
1493 
1494 	spin_lock_irqsave(&camif->slock, flags);
1495 
1496 	switch (ctrl->id) {
1497 	case V4L2_CID_COLORFX:
1498 		camif->colorfx = camif->ctrl_colorfx->val;
1499 		/* Set Cb, Cr */
1500 		switch (ctrl->val) {
1501 		case V4L2_COLORFX_SEPIA:
1502 			camif->colorfx_cb = 115;
1503 			camif->colorfx_cr = 145;
1504 			break;
1505 		case V4L2_COLORFX_SET_CBCR:
1506 			camif->colorfx_cb = camif->ctrl_colorfx_cbcr->val >> 8;
1507 			camif->colorfx_cr = camif->ctrl_colorfx_cbcr->val & 0xff;
1508 			break;
1509 		default:
1510 			/* for V4L2_COLORFX_BW and others */
1511 			camif->colorfx_cb = 128;
1512 			camif->colorfx_cr = 128;
1513 		}
1514 		break;
1515 	case V4L2_CID_TEST_PATTERN:
1516 		camif->test_pattern = camif->ctrl_test_pattern->val;
1517 		break;
1518 	default:
1519 		WARN_ON(1);
1520 	}
1521 
1522 	camif->vp[VP_CODEC].state |= ST_VP_CONFIG;
1523 	camif->vp[VP_PREVIEW].state |= ST_VP_CONFIG;
1524 	spin_unlock_irqrestore(&camif->slock, flags);
1525 
1526 	return 0;
1527 }
1528 
1529 static const struct v4l2_ctrl_ops s3c_camif_subdev_ctrl_ops = {
1530 	.s_ctrl	= s3c_camif_subdev_s_ctrl,
1531 };
1532 
1533 static const char * const s3c_camif_test_pattern_menu[] = {
1534 	"Disabled",
1535 	"Color bars",
1536 	"Horizontal increment",
1537 	"Vertical increment",
1538 };
1539 
1540 int s3c_camif_create_subdev(struct camif_dev *camif)
1541 {
1542 	struct v4l2_ctrl_handler *handler = &camif->ctrl_handler;
1543 	struct v4l2_subdev *sd = &camif->subdev;
1544 	int ret;
1545 
1546 	v4l2_subdev_init(sd, &s3c_camif_subdev_ops);
1547 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1548 	strscpy(sd->name, "S3C-CAMIF", sizeof(sd->name));
1549 
1550 	camif->pads[CAMIF_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1551 	camif->pads[CAMIF_SD_PAD_SOURCE_C].flags = MEDIA_PAD_FL_SOURCE;
1552 	camif->pads[CAMIF_SD_PAD_SOURCE_P].flags = MEDIA_PAD_FL_SOURCE;
1553 
1554 	ret = media_entity_pads_init(&sd->entity, CAMIF_SD_PADS_NUM,
1555 				camif->pads);
1556 	if (ret)
1557 		return ret;
1558 
1559 	v4l2_ctrl_handler_init(handler, 3);
1560 	camif->ctrl_test_pattern = v4l2_ctrl_new_std_menu_items(handler,
1561 			&s3c_camif_subdev_ctrl_ops, V4L2_CID_TEST_PATTERN,
1562 			ARRAY_SIZE(s3c_camif_test_pattern_menu) - 1, 0, 0,
1563 			s3c_camif_test_pattern_menu);
1564 
1565 	if (camif->variant->has_img_effect) {
1566 		camif->ctrl_colorfx = v4l2_ctrl_new_std_menu(handler,
1567 				&s3c_camif_subdev_ctrl_ops,
1568 				V4L2_CID_COLORFX, V4L2_COLORFX_SET_CBCR,
1569 				~0x981f, V4L2_COLORFX_NONE);
1570 
1571 		camif->ctrl_colorfx_cbcr = v4l2_ctrl_new_std(handler,
1572 				&s3c_camif_subdev_ctrl_ops,
1573 				V4L2_CID_COLORFX_CBCR, 0, 0xffff, 1, 0);
1574 	}
1575 
1576 	if (handler->error) {
1577 		v4l2_ctrl_handler_free(handler);
1578 		media_entity_cleanup(&sd->entity);
1579 		return handler->error;
1580 	}
1581 
1582 	if (camif->variant->has_img_effect)
1583 		v4l2_ctrl_auto_cluster(2, &camif->ctrl_colorfx,
1584 			       V4L2_COLORFX_SET_CBCR, false);
1585 
1586 	sd->ctrl_handler = handler;
1587 	v4l2_set_subdevdata(sd, camif);
1588 
1589 	return 0;
1590 }
1591 
1592 void s3c_camif_unregister_subdev(struct camif_dev *camif)
1593 {
1594 	struct v4l2_subdev *sd = &camif->subdev;
1595 
1596 	/* Return if not registered */
1597 	if (v4l2_get_subdevdata(sd) == NULL)
1598 		return;
1599 
1600 	v4l2_device_unregister_subdev(sd);
1601 	media_entity_cleanup(&sd->entity);
1602 	v4l2_ctrl_handler_free(&camif->ctrl_handler);
1603 	v4l2_set_subdevdata(sd, NULL);
1604 }
1605 
1606 int s3c_camif_set_defaults(struct camif_dev *camif)
1607 {
1608 	unsigned int ip_rev = camif->variant->ip_revision;
1609 	int i;
1610 
1611 	for (i = 0; i < CAMIF_VP_NUM; i++) {
1612 		struct camif_vp *vp = &camif->vp[i];
1613 		struct camif_frame *f = &vp->out_frame;
1614 
1615 		vp->camif = camif;
1616 		vp->id = i;
1617 		vp->offset = camif->variant->vp_offset;
1618 
1619 		if (ip_rev == S3C244X_CAMIF_IP_REV)
1620 			vp->fmt_flags = i ? FMT_FL_S3C24XX_PREVIEW :
1621 					FMT_FL_S3C24XX_CODEC;
1622 		else
1623 			vp->fmt_flags = FMT_FL_S3C64XX;
1624 
1625 		vp->out_fmt = s3c_camif_find_format(vp, NULL, 0);
1626 		BUG_ON(vp->out_fmt == NULL);
1627 
1628 		memset(f, 0, sizeof(*f));
1629 		f->f_width = CAMIF_DEF_WIDTH;
1630 		f->f_height = CAMIF_DEF_HEIGHT;
1631 		f->rect.width = CAMIF_DEF_WIDTH;
1632 		f->rect.height = CAMIF_DEF_HEIGHT;
1633 
1634 		/* Scaler is always enabled */
1635 		vp->scaler.enable = 1;
1636 
1637 		vp->payload = (f->f_width * f->f_height *
1638 			       vp->out_fmt->depth) / 8;
1639 	}
1640 
1641 	memset(&camif->mbus_fmt, 0, sizeof(camif->mbus_fmt));
1642 	camif->mbus_fmt.width = CAMIF_DEF_WIDTH;
1643 	camif->mbus_fmt.height = CAMIF_DEF_HEIGHT;
1644 	camif->mbus_fmt.code  = camif_mbus_formats[0];
1645 
1646 	memset(&camif->camif_crop, 0, sizeof(camif->camif_crop));
1647 	camif->camif_crop.width = CAMIF_DEF_WIDTH;
1648 	camif->camif_crop.height = CAMIF_DEF_HEIGHT;
1649 
1650 	return 0;
1651 }
1652