1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Allwinner sun8i DE2 rotation driver
4  *
5  * Copyright (C) 2020 Jernej Skrabec <jernej.skrabec@siol.net>
6  */
7 
8 #include <linux/clk.h>
9 #include <linux/interrupt.h>
10 #include <linux/io.h>
11 #include <linux/iopoll.h>
12 #include <linux/mod_devicetable.h>
13 #include <linux/module.h>
14 #include <linux/platform_device.h>
15 #include <linux/pm_runtime.h>
16 #include <linux/reset.h>
17 
18 #include <media/v4l2-device.h>
19 #include <media/v4l2-event.h>
20 #include <media/v4l2-ioctl.h>
21 #include <media/v4l2-mem2mem.h>
22 
23 #include "sun8i-formats.h"
24 #include "sun8i-rotate.h"
25 
rotate_read(struct rotate_dev * dev,u32 reg)26 static inline u32 rotate_read(struct rotate_dev *dev, u32 reg)
27 {
28 	return readl(dev->base + reg);
29 }
30 
rotate_write(struct rotate_dev * dev,u32 reg,u32 value)31 static inline void rotate_write(struct rotate_dev *dev, u32 reg, u32 value)
32 {
33 	writel(value, dev->base + reg);
34 }
35 
rotate_set_bits(struct rotate_dev * dev,u32 reg,u32 bits)36 static inline void rotate_set_bits(struct rotate_dev *dev, u32 reg, u32 bits)
37 {
38 	writel(readl(dev->base + reg) | bits, dev->base + reg);
39 }
40 
rotate_calc_addr_pitch(dma_addr_t buffer,u32 bytesperline,u32 height,const struct rotate_format * fmt,dma_addr_t * addr,u32 * pitch)41 static void rotate_calc_addr_pitch(dma_addr_t buffer,
42 				   u32 bytesperline, u32 height,
43 				   const struct rotate_format *fmt,
44 				   dma_addr_t *addr, u32 *pitch)
45 {
46 	u32 size;
47 	int i;
48 
49 	for (i = 0; i < fmt->planes; i++) {
50 		pitch[i] = bytesperline;
51 		addr[i] = buffer;
52 		if (i > 0)
53 			pitch[i] /= fmt->hsub / fmt->bpp[i];
54 		size = pitch[i] * height;
55 		if (i > 0)
56 			size /= fmt->vsub;
57 		buffer += size;
58 	}
59 }
60 
rotate_device_run(void * priv)61 static void rotate_device_run(void *priv)
62 {
63 	struct rotate_ctx *ctx = priv;
64 	struct rotate_dev *dev = ctx->dev;
65 	struct vb2_v4l2_buffer *src, *dst;
66 	const struct rotate_format *fmt;
67 	dma_addr_t addr[3];
68 	u32 val, pitch[3];
69 
70 	src = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
71 	dst = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
72 
73 	v4l2_m2m_buf_copy_metadata(src, dst, true);
74 
75 	val = ROTATE_GLB_CTL_MODE(ROTATE_MODE_COPY_ROTATE);
76 	if (ctx->hflip)
77 		val |= ROTATE_GLB_CTL_HFLIP;
78 	if (ctx->vflip)
79 		val |= ROTATE_GLB_CTL_VFLIP;
80 	val |= ROTATE_GLB_CTL_ROTATION(ctx->rotate / 90);
81 	if (ctx->rotate != 90 && ctx->rotate != 270)
82 		val |= ROTATE_GLB_CTL_BURST_LEN(ROTATE_BURST_64);
83 	else
84 		val |= ROTATE_GLB_CTL_BURST_LEN(ROTATE_BURST_8);
85 	rotate_write(dev, ROTATE_GLB_CTL, val);
86 
87 	fmt = rotate_find_format(ctx->src_fmt.pixelformat);
88 	if (!fmt)
89 		return;
90 
91 	rotate_write(dev, ROTATE_IN_FMT, ROTATE_IN_FMT_FORMAT(fmt->hw_format));
92 
93 	rotate_calc_addr_pitch(vb2_dma_contig_plane_dma_addr(&src->vb2_buf, 0),
94 			       ctx->src_fmt.bytesperline, ctx->src_fmt.height,
95 			       fmt, addr, pitch);
96 
97 	rotate_write(dev, ROTATE_IN_SIZE,
98 		     ROTATE_SIZE(ctx->src_fmt.width, ctx->src_fmt.height));
99 
100 	rotate_write(dev, ROTATE_IN_PITCH0, pitch[0]);
101 	rotate_write(dev, ROTATE_IN_PITCH1, pitch[1]);
102 	rotate_write(dev, ROTATE_IN_PITCH2, pitch[2]);
103 
104 	rotate_write(dev, ROTATE_IN_ADDRL0, addr[0]);
105 	rotate_write(dev, ROTATE_IN_ADDRL1, addr[1]);
106 	rotate_write(dev, ROTATE_IN_ADDRL2, addr[2]);
107 
108 	rotate_write(dev, ROTATE_IN_ADDRH0, 0);
109 	rotate_write(dev, ROTATE_IN_ADDRH1, 0);
110 	rotate_write(dev, ROTATE_IN_ADDRH2, 0);
111 
112 	fmt = rotate_find_format(ctx->dst_fmt.pixelformat);
113 	if (!fmt)
114 		return;
115 
116 	rotate_calc_addr_pitch(vb2_dma_contig_plane_dma_addr(&dst->vb2_buf, 0),
117 			       ctx->dst_fmt.bytesperline, ctx->dst_fmt.height,
118 			       fmt, addr, pitch);
119 
120 	rotate_write(dev, ROTATE_OUT_SIZE,
121 		     ROTATE_SIZE(ctx->dst_fmt.width, ctx->dst_fmt.height));
122 
123 	rotate_write(dev, ROTATE_OUT_PITCH0, pitch[0]);
124 	rotate_write(dev, ROTATE_OUT_PITCH1, pitch[1]);
125 	rotate_write(dev, ROTATE_OUT_PITCH2, pitch[2]);
126 
127 	rotate_write(dev, ROTATE_OUT_ADDRL0, addr[0]);
128 	rotate_write(dev, ROTATE_OUT_ADDRL1, addr[1]);
129 	rotate_write(dev, ROTATE_OUT_ADDRL2, addr[2]);
130 
131 	rotate_write(dev, ROTATE_OUT_ADDRH0, 0);
132 	rotate_write(dev, ROTATE_OUT_ADDRH1, 0);
133 	rotate_write(dev, ROTATE_OUT_ADDRH2, 0);
134 
135 	rotate_set_bits(dev, ROTATE_INT, ROTATE_INT_FINISH_IRQ_EN);
136 	rotate_set_bits(dev, ROTATE_GLB_CTL, ROTATE_GLB_CTL_START);
137 }
138 
rotate_irq(int irq,void * data)139 static irqreturn_t rotate_irq(int irq, void *data)
140 {
141 	struct vb2_v4l2_buffer *buffer;
142 	struct rotate_dev *dev = data;
143 	struct rotate_ctx *ctx;
144 	unsigned int val;
145 
146 	ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
147 	if (!ctx) {
148 		v4l2_err(&dev->v4l2_dev,
149 			 "Instance released before the end of transaction\n");
150 		return IRQ_NONE;
151 	}
152 
153 	val = rotate_read(dev, ROTATE_INT);
154 	if (!(val & ROTATE_INT_FINISH_IRQ))
155 		return IRQ_NONE;
156 
157 	/* clear flag and disable irq */
158 	rotate_write(dev, ROTATE_INT, ROTATE_INT_FINISH_IRQ);
159 
160 	buffer = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
161 	v4l2_m2m_buf_done(buffer, VB2_BUF_STATE_DONE);
162 
163 	buffer = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
164 	v4l2_m2m_buf_done(buffer, VB2_BUF_STATE_DONE);
165 
166 	v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
167 
168 	return IRQ_HANDLED;
169 }
170 
rotate_file2ctx(struct file * file)171 static inline struct rotate_ctx *rotate_file2ctx(struct file *file)
172 {
173 	return container_of(file->private_data, struct rotate_ctx, fh);
174 }
175 
rotate_prepare_format(struct v4l2_pix_format * pix_fmt)176 static void rotate_prepare_format(struct v4l2_pix_format *pix_fmt)
177 {
178 	unsigned int height, width, alignment, sizeimage, size, bpl;
179 	const struct rotate_format *fmt;
180 	int i;
181 
182 	fmt = rotate_find_format(pix_fmt->pixelformat);
183 	if (!fmt)
184 		return;
185 
186 	width = ALIGN(pix_fmt->width, fmt->hsub);
187 	height = ALIGN(pix_fmt->height, fmt->vsub);
188 
189 	/* all pitches have to be 16 byte aligned */
190 	alignment = 16;
191 	if (fmt->planes > 1)
192 		alignment *= fmt->hsub / fmt->bpp[1];
193 	bpl = ALIGN(width * fmt->bpp[0], alignment);
194 
195 	sizeimage = 0;
196 	for (i = 0; i < fmt->planes; i++) {
197 		size = bpl * height;
198 		if (i > 0) {
199 			size *= fmt->bpp[i];
200 			size /= fmt->hsub;
201 			size /= fmt->vsub;
202 		}
203 		sizeimage += size;
204 	}
205 
206 	pix_fmt->width = width;
207 	pix_fmt->height = height;
208 	pix_fmt->bytesperline = bpl;
209 	pix_fmt->sizeimage = sizeimage;
210 }
211 
rotate_querycap(struct file * file,void * priv,struct v4l2_capability * cap)212 static int rotate_querycap(struct file *file, void *priv,
213 			   struct v4l2_capability *cap)
214 {
215 	strscpy(cap->driver, ROTATE_NAME, sizeof(cap->driver));
216 	strscpy(cap->card, ROTATE_NAME, sizeof(cap->card));
217 	snprintf(cap->bus_info, sizeof(cap->bus_info),
218 		 "platform:%s", ROTATE_NAME);
219 
220 	return 0;
221 }
222 
rotate_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)223 static int rotate_enum_fmt_vid_cap(struct file *file, void *priv,
224 				   struct v4l2_fmtdesc *f)
225 {
226 	return rotate_enum_fmt(f, true);
227 }
228 
rotate_enum_fmt_vid_out(struct file * file,void * priv,struct v4l2_fmtdesc * f)229 static int rotate_enum_fmt_vid_out(struct file *file, void *priv,
230 				   struct v4l2_fmtdesc *f)
231 {
232 	return rotate_enum_fmt(f, false);
233 }
234 
rotate_enum_framesizes(struct file * file,void * priv,struct v4l2_frmsizeenum * fsize)235 static int rotate_enum_framesizes(struct file *file, void *priv,
236 				  struct v4l2_frmsizeenum *fsize)
237 {
238 	const struct rotate_format *fmt;
239 
240 	if (fsize->index != 0)
241 		return -EINVAL;
242 
243 	fmt = rotate_find_format(fsize->pixel_format);
244 	if (!fmt)
245 		return -EINVAL;
246 
247 	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
248 	fsize->stepwise.min_width = ROTATE_MIN_WIDTH;
249 	fsize->stepwise.min_height = ROTATE_MIN_HEIGHT;
250 	fsize->stepwise.max_width = ROTATE_MAX_WIDTH;
251 	fsize->stepwise.max_height = ROTATE_MAX_HEIGHT;
252 	fsize->stepwise.step_width = fmt->hsub;
253 	fsize->stepwise.step_height = fmt->vsub;
254 
255 	return 0;
256 }
257 
rotate_set_cap_format(struct rotate_ctx * ctx,struct v4l2_pix_format * f,u32 rotate)258 static int rotate_set_cap_format(struct rotate_ctx *ctx,
259 				 struct v4l2_pix_format *f,
260 				 u32 rotate)
261 {
262 	const struct rotate_format *fmt;
263 
264 	fmt = rotate_find_format(ctx->src_fmt.pixelformat);
265 	if (!fmt)
266 		return -EINVAL;
267 
268 	if (fmt->flags & ROTATE_FLAG_YUV)
269 		f->pixelformat = V4L2_PIX_FMT_YUV420;
270 	else
271 		f->pixelformat = ctx->src_fmt.pixelformat;
272 
273 	f->field = V4L2_FIELD_NONE;
274 
275 	if (rotate == 90 || rotate == 270) {
276 		f->width = ctx->src_fmt.height;
277 		f->height = ctx->src_fmt.width;
278 	} else {
279 		f->width = ctx->src_fmt.width;
280 		f->height = ctx->src_fmt.height;
281 	}
282 
283 	rotate_prepare_format(f);
284 
285 	return 0;
286 }
287 
rotate_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)288 static int rotate_g_fmt_vid_cap(struct file *file, void *priv,
289 				struct v4l2_format *f)
290 {
291 	struct rotate_ctx *ctx = rotate_file2ctx(file);
292 
293 	f->fmt.pix = ctx->dst_fmt;
294 
295 	return 0;
296 }
297 
rotate_g_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)298 static int rotate_g_fmt_vid_out(struct file *file, void *priv,
299 				struct v4l2_format *f)
300 {
301 	struct rotate_ctx *ctx = rotate_file2ctx(file);
302 
303 	f->fmt.pix = ctx->src_fmt;
304 
305 	return 0;
306 }
307 
rotate_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)308 static int rotate_try_fmt_vid_cap(struct file *file, void *priv,
309 				  struct v4l2_format *f)
310 {
311 	struct rotate_ctx *ctx = rotate_file2ctx(file);
312 
313 	return rotate_set_cap_format(ctx, &f->fmt.pix, ctx->rotate);
314 }
315 
rotate_try_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)316 static int rotate_try_fmt_vid_out(struct file *file, void *priv,
317 				  struct v4l2_format *f)
318 {
319 	if (!rotate_find_format(f->fmt.pix.pixelformat))
320 		f->fmt.pix.pixelformat = V4L2_PIX_FMT_ARGB32;
321 
322 	if (f->fmt.pix.width < ROTATE_MIN_WIDTH)
323 		f->fmt.pix.width = ROTATE_MIN_WIDTH;
324 	if (f->fmt.pix.height < ROTATE_MIN_HEIGHT)
325 		f->fmt.pix.height = ROTATE_MIN_HEIGHT;
326 
327 	if (f->fmt.pix.width > ROTATE_MAX_WIDTH)
328 		f->fmt.pix.width = ROTATE_MAX_WIDTH;
329 	if (f->fmt.pix.height > ROTATE_MAX_HEIGHT)
330 		f->fmt.pix.height = ROTATE_MAX_HEIGHT;
331 
332 	f->fmt.pix.field = V4L2_FIELD_NONE;
333 
334 	rotate_prepare_format(&f->fmt.pix);
335 
336 	return 0;
337 }
338 
rotate_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)339 static int rotate_s_fmt_vid_cap(struct file *file, void *priv,
340 				struct v4l2_format *f)
341 {
342 	struct rotate_ctx *ctx = rotate_file2ctx(file);
343 	struct vb2_queue *vq;
344 	int ret;
345 
346 	ret = rotate_try_fmt_vid_cap(file, priv, f);
347 	if (ret)
348 		return ret;
349 
350 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
351 	if (vb2_is_busy(vq))
352 		return -EBUSY;
353 
354 	ctx->dst_fmt = f->fmt.pix;
355 
356 	return 0;
357 }
358 
rotate_s_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)359 static int rotate_s_fmt_vid_out(struct file *file, void *priv,
360 				struct v4l2_format *f)
361 {
362 	struct rotate_ctx *ctx = rotate_file2ctx(file);
363 	struct vb2_queue *vq;
364 	int ret;
365 
366 	ret = rotate_try_fmt_vid_out(file, priv, f);
367 	if (ret)
368 		return ret;
369 
370 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
371 	if (vb2_is_busy(vq))
372 		return -EBUSY;
373 
374 	/*
375 	 * Capture queue has to be also checked, because format and size
376 	 * depends on output format and size.
377 	 */
378 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
379 	if (vb2_is_busy(vq))
380 		return -EBUSY;
381 
382 	ctx->src_fmt = f->fmt.pix;
383 
384 	/* Propagate colorspace information to capture. */
385 	ctx->dst_fmt.colorspace = f->fmt.pix.colorspace;
386 	ctx->dst_fmt.xfer_func = f->fmt.pix.xfer_func;
387 	ctx->dst_fmt.ycbcr_enc = f->fmt.pix.ycbcr_enc;
388 	ctx->dst_fmt.quantization = f->fmt.pix.quantization;
389 
390 	return rotate_set_cap_format(ctx, &ctx->dst_fmt, ctx->rotate);
391 }
392 
393 static const struct v4l2_ioctl_ops rotate_ioctl_ops = {
394 	.vidioc_querycap		= rotate_querycap,
395 
396 	.vidioc_enum_framesizes		= rotate_enum_framesizes,
397 
398 	.vidioc_enum_fmt_vid_cap	= rotate_enum_fmt_vid_cap,
399 	.vidioc_g_fmt_vid_cap		= rotate_g_fmt_vid_cap,
400 	.vidioc_try_fmt_vid_cap		= rotate_try_fmt_vid_cap,
401 	.vidioc_s_fmt_vid_cap		= rotate_s_fmt_vid_cap,
402 
403 	.vidioc_enum_fmt_vid_out	= rotate_enum_fmt_vid_out,
404 	.vidioc_g_fmt_vid_out		= rotate_g_fmt_vid_out,
405 	.vidioc_try_fmt_vid_out		= rotate_try_fmt_vid_out,
406 	.vidioc_s_fmt_vid_out		= rotate_s_fmt_vid_out,
407 
408 	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
409 	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
410 	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
411 	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
412 	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
413 	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
414 	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
415 
416 	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
417 	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
418 
419 	.vidioc_log_status		= v4l2_ctrl_log_status,
420 	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
421 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
422 };
423 
rotate_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])424 static int rotate_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
425 			      unsigned int *nplanes, unsigned int sizes[],
426 			      struct device *alloc_devs[])
427 {
428 	struct rotate_ctx *ctx = vb2_get_drv_priv(vq);
429 	struct v4l2_pix_format *pix_fmt;
430 
431 	if (V4L2_TYPE_IS_OUTPUT(vq->type))
432 		pix_fmt = &ctx->src_fmt;
433 	else
434 		pix_fmt = &ctx->dst_fmt;
435 
436 	if (*nplanes) {
437 		if (sizes[0] < pix_fmt->sizeimage)
438 			return -EINVAL;
439 	} else {
440 		sizes[0] = pix_fmt->sizeimage;
441 		*nplanes = 1;
442 	}
443 
444 	return 0;
445 }
446 
rotate_buf_prepare(struct vb2_buffer * vb)447 static int rotate_buf_prepare(struct vb2_buffer *vb)
448 {
449 	struct vb2_queue *vq = vb->vb2_queue;
450 	struct rotate_ctx *ctx = vb2_get_drv_priv(vq);
451 	struct v4l2_pix_format *pix_fmt;
452 
453 	if (V4L2_TYPE_IS_OUTPUT(vq->type))
454 		pix_fmt = &ctx->src_fmt;
455 	else
456 		pix_fmt = &ctx->dst_fmt;
457 
458 	if (vb2_plane_size(vb, 0) < pix_fmt->sizeimage)
459 		return -EINVAL;
460 
461 	vb2_set_plane_payload(vb, 0, pix_fmt->sizeimage);
462 
463 	return 0;
464 }
465 
rotate_buf_queue(struct vb2_buffer * vb)466 static void rotate_buf_queue(struct vb2_buffer *vb)
467 {
468 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
469 	struct rotate_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
470 
471 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
472 }
473 
rotate_queue_cleanup(struct vb2_queue * vq,u32 state)474 static void rotate_queue_cleanup(struct vb2_queue *vq, u32 state)
475 {
476 	struct rotate_ctx *ctx = vb2_get_drv_priv(vq);
477 	struct vb2_v4l2_buffer *vbuf;
478 
479 	do {
480 		if (V4L2_TYPE_IS_OUTPUT(vq->type))
481 			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
482 		else
483 			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
484 
485 		if (vbuf)
486 			v4l2_m2m_buf_done(vbuf, state);
487 	} while (vbuf);
488 }
489 
rotate_start_streaming(struct vb2_queue * vq,unsigned int count)490 static int rotate_start_streaming(struct vb2_queue *vq, unsigned int count)
491 {
492 	if (V4L2_TYPE_IS_OUTPUT(vq->type)) {
493 		struct rotate_ctx *ctx = vb2_get_drv_priv(vq);
494 		struct device *dev = ctx->dev->dev;
495 		int ret;
496 
497 		ret = pm_runtime_resume_and_get(dev);
498 		if (ret < 0) {
499 			dev_err(dev, "Failed to enable module\n");
500 
501 			return ret;
502 		}
503 	}
504 
505 	return 0;
506 }
507 
rotate_stop_streaming(struct vb2_queue * vq)508 static void rotate_stop_streaming(struct vb2_queue *vq)
509 {
510 	if (V4L2_TYPE_IS_OUTPUT(vq->type)) {
511 		struct rotate_ctx *ctx = vb2_get_drv_priv(vq);
512 
513 		pm_runtime_put(ctx->dev->dev);
514 	}
515 
516 	rotate_queue_cleanup(vq, VB2_BUF_STATE_ERROR);
517 }
518 
519 static const struct vb2_ops rotate_qops = {
520 	.queue_setup		= rotate_queue_setup,
521 	.buf_prepare		= rotate_buf_prepare,
522 	.buf_queue		= rotate_buf_queue,
523 	.start_streaming	= rotate_start_streaming,
524 	.stop_streaming		= rotate_stop_streaming,
525 	.wait_prepare		= vb2_ops_wait_prepare,
526 	.wait_finish		= vb2_ops_wait_finish,
527 };
528 
rotate_queue_init(void * priv,struct vb2_queue * src_vq,struct vb2_queue * dst_vq)529 static int rotate_queue_init(void *priv, struct vb2_queue *src_vq,
530 			     struct vb2_queue *dst_vq)
531 {
532 	struct rotate_ctx *ctx = priv;
533 	int ret;
534 
535 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
536 	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
537 	src_vq->drv_priv = ctx;
538 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
539 	src_vq->min_buffers_needed = 1;
540 	src_vq->ops = &rotate_qops;
541 	src_vq->mem_ops = &vb2_dma_contig_memops;
542 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
543 	src_vq->lock = &ctx->dev->dev_mutex;
544 	src_vq->dev = ctx->dev->dev;
545 
546 	ret = vb2_queue_init(src_vq);
547 	if (ret)
548 		return ret;
549 
550 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
551 	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
552 	dst_vq->drv_priv = ctx;
553 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
554 	dst_vq->min_buffers_needed = 2;
555 	dst_vq->ops = &rotate_qops;
556 	dst_vq->mem_ops = &vb2_dma_contig_memops;
557 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
558 	dst_vq->lock = &ctx->dev->dev_mutex;
559 	dst_vq->dev = ctx->dev->dev;
560 
561 	ret = vb2_queue_init(dst_vq);
562 	if (ret)
563 		return ret;
564 
565 	return 0;
566 }
567 
rotate_s_ctrl(struct v4l2_ctrl * ctrl)568 static int rotate_s_ctrl(struct v4l2_ctrl *ctrl)
569 {
570 	struct rotate_ctx *ctx = container_of(ctrl->handler,
571 					      struct rotate_ctx,
572 					      ctrl_handler);
573 	struct v4l2_pix_format fmt;
574 
575 	switch (ctrl->id) {
576 	case V4L2_CID_HFLIP:
577 		ctx->hflip = ctrl->val;
578 		break;
579 	case V4L2_CID_VFLIP:
580 		ctx->vflip = ctrl->val;
581 		break;
582 	case V4L2_CID_ROTATE:
583 		rotate_set_cap_format(ctx, &fmt, ctrl->val);
584 
585 		/* Check if capture format needs to be changed */
586 		if (fmt.width != ctx->dst_fmt.width ||
587 		    fmt.height != ctx->dst_fmt.height ||
588 		    fmt.bytesperline != ctx->dst_fmt.bytesperline ||
589 		    fmt.sizeimage != ctx->dst_fmt.sizeimage) {
590 			struct vb2_queue *vq;
591 
592 			vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
593 					     V4L2_BUF_TYPE_VIDEO_CAPTURE);
594 			if (vb2_is_busy(vq))
595 				return -EBUSY;
596 
597 			rotate_set_cap_format(ctx, &ctx->dst_fmt, ctrl->val);
598 		}
599 
600 		ctx->rotate = ctrl->val;
601 		break;
602 	default:
603 		return -EINVAL;
604 	}
605 
606 	return 0;
607 }
608 
609 static const struct v4l2_ctrl_ops rotate_ctrl_ops = {
610 	.s_ctrl = rotate_s_ctrl,
611 };
612 
rotate_setup_ctrls(struct rotate_ctx * ctx)613 static int rotate_setup_ctrls(struct rotate_ctx *ctx)
614 {
615 	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 3);
616 
617 	v4l2_ctrl_new_std(&ctx->ctrl_handler, &rotate_ctrl_ops,
618 			  V4L2_CID_HFLIP, 0, 1, 1, 0);
619 
620 	v4l2_ctrl_new_std(&ctx->ctrl_handler, &rotate_ctrl_ops,
621 			  V4L2_CID_VFLIP, 0, 1, 1, 0);
622 
623 	v4l2_ctrl_new_std(&ctx->ctrl_handler, &rotate_ctrl_ops,
624 			  V4L2_CID_ROTATE, 0, 270, 90, 0);
625 
626 	if (ctx->ctrl_handler.error) {
627 		int err = ctx->ctrl_handler.error;
628 
629 		v4l2_err(&ctx->dev->v4l2_dev, "control setup failed!\n");
630 		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
631 
632 		return err;
633 	}
634 
635 	return v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
636 }
637 
rotate_open(struct file * file)638 static int rotate_open(struct file *file)
639 {
640 	struct rotate_dev *dev = video_drvdata(file);
641 	struct rotate_ctx *ctx = NULL;
642 	int ret;
643 
644 	if (mutex_lock_interruptible(&dev->dev_mutex))
645 		return -ERESTARTSYS;
646 
647 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
648 	if (!ctx) {
649 		mutex_unlock(&dev->dev_mutex);
650 		return -ENOMEM;
651 	}
652 
653 	/* default output format */
654 	ctx->src_fmt.pixelformat = V4L2_PIX_FMT_ARGB32;
655 	ctx->src_fmt.field = V4L2_FIELD_NONE;
656 	ctx->src_fmt.width = 640;
657 	ctx->src_fmt.height = 480;
658 	rotate_prepare_format(&ctx->src_fmt);
659 
660 	/* default capture format */
661 	rotate_set_cap_format(ctx, &ctx->dst_fmt, ctx->rotate);
662 
663 	v4l2_fh_init(&ctx->fh, video_devdata(file));
664 	file->private_data = &ctx->fh;
665 	ctx->dev = dev;
666 
667 	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx,
668 					    &rotate_queue_init);
669 	if (IS_ERR(ctx->fh.m2m_ctx)) {
670 		ret = PTR_ERR(ctx->fh.m2m_ctx);
671 		goto err_free;
672 	}
673 
674 	v4l2_fh_add(&ctx->fh);
675 
676 	ret = rotate_setup_ctrls(ctx);
677 	if (ret)
678 		goto err_free;
679 
680 	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
681 
682 	mutex_unlock(&dev->dev_mutex);
683 
684 	return 0;
685 
686 err_free:
687 	kfree(ctx);
688 	mutex_unlock(&dev->dev_mutex);
689 
690 	return ret;
691 }
692 
rotate_release(struct file * file)693 static int rotate_release(struct file *file)
694 {
695 	struct rotate_dev *dev = video_drvdata(file);
696 	struct rotate_ctx *ctx = container_of(file->private_data,
697 						   struct rotate_ctx, fh);
698 
699 	mutex_lock(&dev->dev_mutex);
700 
701 	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
702 	v4l2_fh_del(&ctx->fh);
703 	v4l2_fh_exit(&ctx->fh);
704 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
705 
706 	kfree(ctx);
707 
708 	mutex_unlock(&dev->dev_mutex);
709 
710 	return 0;
711 }
712 
713 static const struct v4l2_file_operations rotate_fops = {
714 	.owner		= THIS_MODULE,
715 	.open		= rotate_open,
716 	.release	= rotate_release,
717 	.poll		= v4l2_m2m_fop_poll,
718 	.unlocked_ioctl	= video_ioctl2,
719 	.mmap		= v4l2_m2m_fop_mmap,
720 };
721 
722 static const struct video_device rotate_video_device = {
723 	.name		= ROTATE_NAME,
724 	.vfl_dir	= VFL_DIR_M2M,
725 	.fops		= &rotate_fops,
726 	.ioctl_ops	= &rotate_ioctl_ops,
727 	.minor		= -1,
728 	.release	= video_device_release_empty,
729 	.device_caps	= V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
730 };
731 
732 static const struct v4l2_m2m_ops rotate_m2m_ops = {
733 	.device_run	= rotate_device_run,
734 };
735 
rotate_probe(struct platform_device * pdev)736 static int rotate_probe(struct platform_device *pdev)
737 {
738 	struct rotate_dev *dev;
739 	struct video_device *vfd;
740 	int irq, ret;
741 
742 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
743 	if (!dev)
744 		return -ENOMEM;
745 
746 	dev->vfd = rotate_video_device;
747 	dev->dev = &pdev->dev;
748 
749 	irq = platform_get_irq(pdev, 0);
750 	if (irq <= 0)
751 		return irq;
752 
753 	ret = devm_request_irq(dev->dev, irq, rotate_irq,
754 			       0, dev_name(dev->dev), dev);
755 	if (ret) {
756 		dev_err(dev->dev, "Failed to request IRQ\n");
757 
758 		return ret;
759 	}
760 
761 	dev->base = devm_platform_ioremap_resource(pdev, 0);
762 	if (IS_ERR(dev->base))
763 		return PTR_ERR(dev->base);
764 
765 	dev->bus_clk = devm_clk_get(dev->dev, "bus");
766 	if (IS_ERR(dev->bus_clk)) {
767 		dev_err(dev->dev, "Failed to get bus clock\n");
768 
769 		return PTR_ERR(dev->bus_clk);
770 	}
771 
772 	dev->mod_clk = devm_clk_get(dev->dev, "mod");
773 	if (IS_ERR(dev->mod_clk)) {
774 		dev_err(dev->dev, "Failed to get mod clock\n");
775 
776 		return PTR_ERR(dev->mod_clk);
777 	}
778 
779 	dev->rstc = devm_reset_control_get(dev->dev, NULL);
780 	if (IS_ERR(dev->rstc)) {
781 		dev_err(dev->dev, "Failed to get reset control\n");
782 
783 		return PTR_ERR(dev->rstc);
784 	}
785 
786 	mutex_init(&dev->dev_mutex);
787 
788 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
789 	if (ret) {
790 		dev_err(dev->dev, "Failed to register V4L2 device\n");
791 
792 		return ret;
793 	}
794 
795 	vfd = &dev->vfd;
796 	vfd->lock = &dev->dev_mutex;
797 	vfd->v4l2_dev = &dev->v4l2_dev;
798 
799 	snprintf(vfd->name, sizeof(vfd->name), "%s",
800 		 rotate_video_device.name);
801 	video_set_drvdata(vfd, dev);
802 
803 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
804 	if (ret) {
805 		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
806 
807 		goto err_v4l2;
808 	}
809 
810 	v4l2_info(&dev->v4l2_dev,
811 		  "Device registered as /dev/video%d\n", vfd->num);
812 
813 	dev->m2m_dev = v4l2_m2m_init(&rotate_m2m_ops);
814 	if (IS_ERR(dev->m2m_dev)) {
815 		v4l2_err(&dev->v4l2_dev,
816 			 "Failed to initialize V4L2 M2M device\n");
817 		ret = PTR_ERR(dev->m2m_dev);
818 
819 		goto err_video;
820 	}
821 
822 	platform_set_drvdata(pdev, dev);
823 
824 	pm_runtime_enable(dev->dev);
825 
826 	return 0;
827 
828 err_video:
829 	video_unregister_device(&dev->vfd);
830 err_v4l2:
831 	v4l2_device_unregister(&dev->v4l2_dev);
832 
833 	return ret;
834 }
835 
rotate_remove(struct platform_device * pdev)836 static void rotate_remove(struct platform_device *pdev)
837 {
838 	struct rotate_dev *dev = platform_get_drvdata(pdev);
839 
840 	v4l2_m2m_release(dev->m2m_dev);
841 	video_unregister_device(&dev->vfd);
842 	v4l2_device_unregister(&dev->v4l2_dev);
843 
844 	pm_runtime_force_suspend(&pdev->dev);
845 }
846 
rotate_runtime_resume(struct device * device)847 static int rotate_runtime_resume(struct device *device)
848 {
849 	struct rotate_dev *dev = dev_get_drvdata(device);
850 	int ret;
851 
852 	ret = clk_prepare_enable(dev->bus_clk);
853 	if (ret) {
854 		dev_err(dev->dev, "Failed to enable bus clock\n");
855 
856 		return ret;
857 	}
858 
859 	ret = clk_prepare_enable(dev->mod_clk);
860 	if (ret) {
861 		dev_err(dev->dev, "Failed to enable mod clock\n");
862 
863 		goto err_bus_clk;
864 	}
865 
866 	ret = reset_control_deassert(dev->rstc);
867 	if (ret) {
868 		dev_err(dev->dev, "Failed to apply reset\n");
869 
870 		goto err_mod_clk;
871 	}
872 
873 	return 0;
874 
875 err_mod_clk:
876 	clk_disable_unprepare(dev->mod_clk);
877 err_bus_clk:
878 	clk_disable_unprepare(dev->bus_clk);
879 
880 	return ret;
881 }
882 
rotate_runtime_suspend(struct device * device)883 static int rotate_runtime_suspend(struct device *device)
884 {
885 	struct rotate_dev *dev = dev_get_drvdata(device);
886 
887 	reset_control_assert(dev->rstc);
888 
889 	clk_disable_unprepare(dev->mod_clk);
890 	clk_disable_unprepare(dev->bus_clk);
891 
892 	return 0;
893 }
894 
895 static const struct of_device_id rotate_dt_match[] = {
896 	{ .compatible = "allwinner,sun8i-a83t-de2-rotate" },
897 	{ /* sentinel */ }
898 };
899 MODULE_DEVICE_TABLE(of, rotate_dt_match);
900 
901 static const struct dev_pm_ops rotate_pm_ops = {
902 	.runtime_resume		= rotate_runtime_resume,
903 	.runtime_suspend	= rotate_runtime_suspend,
904 };
905 
906 static struct platform_driver rotate_driver = {
907 	.probe		= rotate_probe,
908 	.remove_new	= rotate_remove,
909 	.driver		= {
910 		.name		= ROTATE_NAME,
911 		.of_match_table	= rotate_dt_match,
912 		.pm		= &rotate_pm_ops,
913 	},
914 };
915 module_platform_driver(rotate_driver);
916 
917 MODULE_LICENSE("GPL v2");
918 MODULE_AUTHOR("Jernej Skrabec <jernej.skrabec@siol.net>");
919 MODULE_DESCRIPTION("Allwinner DE2 rotate driver");
920