1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * stf_capture.c
4  *
5  * StarFive Camera Subsystem - capture device
6  *
7  * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
8  */
9 
10 #include "stf-camss.h"
11 
12 static const char * const stf_cap_names[] = {
13 	"capture_raw",
14 	"capture_yuv",
15 };
16 
17 static const struct stfcamss_format_info stf_wr_fmts[] = {
18 	{
19 		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
20 		.pixelformat = V4L2_PIX_FMT_SRGGB10,
21 		.planes = 1,
22 		.vsub = { 1 },
23 		.bpp = 16,
24 	},
25 	{
26 		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
27 		.pixelformat = V4L2_PIX_FMT_SGRBG10,
28 		.planes = 1,
29 		.vsub = { 1 },
30 		.bpp = 16,
31 	},
32 	{
33 		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
34 		.pixelformat = V4L2_PIX_FMT_SGBRG10,
35 		.planes = 1,
36 		.vsub = { 1 },
37 		.bpp = 16,
38 	},
39 	{
40 		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
41 		.pixelformat = V4L2_PIX_FMT_SBGGR10,
42 		.planes = 1,
43 		.vsub = { 1 },
44 		.bpp = 16,
45 	},
46 };
47 
48 static const struct stfcamss_format_info stf_isp_fmts[] = {
49 	{
50 		.code = MEDIA_BUS_FMT_YUYV8_1_5X8,
51 		.pixelformat = V4L2_PIX_FMT_NV12,
52 		.planes = 2,
53 		.vsub = { 1, 2 },
54 		.bpp = 8,
55 	},
56 };
57 
58 static inline struct stf_capture *to_stf_capture(struct stfcamss_video *video)
59 {
60 	return container_of(video, struct stf_capture, video);
61 }
62 
63 static void stf_set_raw_addr(struct stfcamss *stfcamss, dma_addr_t addr)
64 {
65 	stf_syscon_reg_write(stfcamss, VIN_START_ADDR_O, (long)addr);
66 	stf_syscon_reg_write(stfcamss, VIN_START_ADDR_N, (long)addr);
67 }
68 
69 static void stf_set_yuv_addr(struct stfcamss *stfcamss,
70 			     dma_addr_t y_addr, dma_addr_t uv_addr)
71 {
72 	stf_isp_reg_write(stfcamss, ISP_REG_Y_PLANE_START_ADDR, y_addr);
73 	stf_isp_reg_write(stfcamss, ISP_REG_UV_PLANE_START_ADDR, uv_addr);
74 }
75 
76 static void stf_init_addrs(struct stfcamss_video *video)
77 {
78 	struct stf_capture *cap = to_stf_capture(video);
79 	struct stf_v_buf *output = &cap->buffers;
80 	dma_addr_t addr0, addr1;
81 
82 	output->active_buf = 0;
83 
84 	if (!output->buf[0])
85 		return;
86 
87 	addr0 = output->buf[0]->addr[0];
88 	addr1 = output->buf[0]->addr[1];
89 
90 	if (cap->type == STF_CAPTURE_RAW)
91 		stf_set_raw_addr(video->stfcamss, addr0);
92 	else if (cap->type == STF_CAPTURE_YUV)
93 		stf_set_yuv_addr(video->stfcamss, addr0, addr1);
94 }
95 
96 static struct stfcamss_buffer *stf_buf_get_pending(struct stf_v_buf *output)
97 {
98 	struct stfcamss_buffer *buffer = NULL;
99 
100 	if (!list_empty(&output->pending_bufs)) {
101 		buffer = list_first_entry(&output->pending_bufs,
102 					  struct stfcamss_buffer,
103 					  queue);
104 		list_del(&buffer->queue);
105 	}
106 
107 	return buffer;
108 }
109 
110 static void stf_cap_s_cfg(struct stfcamss_video *video)
111 {
112 	struct stf_capture *cap = to_stf_capture(video);
113 	struct stf_v_buf *output = &cap->buffers;
114 	unsigned long flags;
115 
116 	spin_lock_irqsave(&output->lock, flags);
117 
118 	output->state = STF_OUTPUT_IDLE;
119 	output->buf[0] = stf_buf_get_pending(output);
120 
121 	if (!output->buf[0] && output->buf[1]) {
122 		output->buf[0] = output->buf[1];
123 		output->buf[1] = NULL;
124 	}
125 
126 	if (output->buf[0])
127 		output->state = STF_OUTPUT_SINGLE;
128 
129 	output->sequence = 0;
130 	stf_init_addrs(video);
131 
132 	spin_unlock_irqrestore(&output->lock, flags);
133 }
134 
135 static int stf_cap_s_cleanup(struct stfcamss_video *video)
136 {
137 	struct stf_capture *cap = to_stf_capture(video);
138 	struct stf_v_buf *output = &cap->buffers;
139 	unsigned long flags;
140 
141 	spin_lock_irqsave(&output->lock, flags);
142 
143 	output->state = STF_OUTPUT_OFF;
144 
145 	spin_unlock_irqrestore(&output->lock, flags);
146 
147 	return 0;
148 }
149 
150 static void stf_wr_data_en(struct stfcamss_video *video)
151 {
152 	struct stf_capture *cap = to_stf_capture(video);
153 	struct stfcamss *stfcamss = cap->video.stfcamss;
154 
155 	stf_syscon_reg_set_bit(stfcamss, VIN_CHANNEL_SEL_EN, U0_VIN_AXIWR0_EN);
156 }
157 
158 static void stf_wr_irq_enable(struct stfcamss_video *video)
159 {
160 	struct stf_capture *cap = to_stf_capture(video);
161 	struct stfcamss *stfcamss = cap->video.stfcamss;
162 
163 	stf_syscon_reg_clear_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_M);
164 }
165 
166 static void stf_wr_irq_disable(struct stfcamss_video *video)
167 {
168 	struct stf_capture *cap = to_stf_capture(video);
169 	struct stfcamss *stfcamss = cap->video.stfcamss;
170 
171 	stf_syscon_reg_set_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN);
172 	stf_syscon_reg_clear_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN);
173 	stf_syscon_reg_set_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_M);
174 }
175 
176 static void stf_channel_set(struct stfcamss_video *video)
177 {
178 	struct stf_capture *cap = to_stf_capture(video);
179 	struct stfcamss *stfcamss = cap->video.stfcamss;
180 	u32 val;
181 
182 	if (cap->type == STF_CAPTURE_RAW) {
183 		val = stf_syscon_reg_read(stfcamss, VIN_CHANNEL_SEL_EN);
184 		val &= ~U0_VIN_CHANNEL_SEL_MASK;
185 		val |= CHANNEL(0);
186 		stf_syscon_reg_write(stfcamss, VIN_CHANNEL_SEL_EN, val);
187 
188 		val = stf_syscon_reg_read(stfcamss, VIN_INRT_PIX_CFG);
189 		val &= ~U0_VIN_PIX_CT_MASK;
190 		val |= PIX_CT(1);
191 
192 		val &= ~U0_VIN_PIXEL_HEIGH_BIT_SEL_MAKS;
193 		val |= PIXEL_HEIGH_BIT_SEL(0);
194 
195 		val &= ~U0_VIN_PIX_CNT_END_MASK;
196 		val |= PIX_CNT_END(IMAGE_MAX_WIDTH / 4 - 1);
197 
198 		stf_syscon_reg_write(stfcamss, VIN_INRT_PIX_CFG, val);
199 	} else if (cap->type == STF_CAPTURE_YUV) {
200 		val = stf_syscon_reg_read(stfcamss, VIN_CFG_REG);
201 		val &= ~U0_VIN_MIPI_BYTE_EN_ISP0_MASK;
202 		val |= U0_VIN_MIPI_BYTE_EN_ISP0(0);
203 
204 		val &= ~U0_VIN_MIPI_CHANNEL_SEL0_MASK;
205 		val |= U0_VIN_MIPI_CHANNEL_SEL0(0);
206 
207 		val &= ~U0_VIN_PIX_NUM_MASK;
208 		val |= U0_VIN_PIX_NUM(0);
209 
210 		val &= ~U0_VIN_P_I_MIPI_HAEDER_EN0_MASK;
211 		val |= U0_VIN_P_I_MIPI_HAEDER_EN0(1);
212 
213 		stf_syscon_reg_write(stfcamss, VIN_CFG_REG, val);
214 	}
215 }
216 
217 static void stf_capture_start(struct stfcamss_video *video)
218 {
219 	struct stf_capture *cap = to_stf_capture(video);
220 
221 	stf_channel_set(video);
222 	if (cap->type == STF_CAPTURE_RAW) {
223 		stf_wr_irq_enable(video);
224 		stf_wr_data_en(video);
225 	}
226 
227 	stf_cap_s_cfg(video);
228 }
229 
230 static void stf_capture_stop(struct stfcamss_video *video)
231 {
232 	struct stf_capture *cap = to_stf_capture(video);
233 
234 	if (cap->type == STF_CAPTURE_RAW)
235 		stf_wr_irq_disable(video);
236 
237 	stf_cap_s_cleanup(video);
238 }
239 
240 static void stf_capture_init(struct stfcamss *stfcamss, struct stf_capture *cap)
241 {
242 	cap->buffers.state = STF_OUTPUT_OFF;
243 	cap->buffers.buf[0] = NULL;
244 	cap->buffers.buf[1] = NULL;
245 	cap->buffers.active_buf = 0;
246 	atomic_set(&cap->buffers.frame_skip, 4);
247 	INIT_LIST_HEAD(&cap->buffers.pending_bufs);
248 	INIT_LIST_HEAD(&cap->buffers.ready_bufs);
249 	spin_lock_init(&cap->buffers.lock);
250 
251 	cap->video.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
252 	cap->video.stfcamss = stfcamss;
253 	cap->video.bpl_alignment = 16 * 8;
254 
255 	if (cap->type == STF_CAPTURE_RAW) {
256 		cap->video.formats = stf_wr_fmts;
257 		cap->video.nformats = ARRAY_SIZE(stf_wr_fmts);
258 		cap->video.bpl_alignment = 8;
259 	} else if (cap->type == STF_CAPTURE_YUV) {
260 		cap->video.formats = stf_isp_fmts;
261 		cap->video.nformats = ARRAY_SIZE(stf_isp_fmts);
262 		cap->video.bpl_alignment = 1;
263 	}
264 }
265 
266 static void stf_buf_add_ready(struct stf_v_buf *output,
267 			      struct stfcamss_buffer *buffer)
268 {
269 	INIT_LIST_HEAD(&buffer->queue);
270 	list_add_tail(&buffer->queue, &output->ready_bufs);
271 }
272 
273 static struct stfcamss_buffer *stf_buf_get_ready(struct stf_v_buf *output)
274 {
275 	struct stfcamss_buffer *buffer = NULL;
276 
277 	if (!list_empty(&output->ready_bufs)) {
278 		buffer = list_first_entry(&output->ready_bufs,
279 					  struct stfcamss_buffer,
280 					  queue);
281 		list_del(&buffer->queue);
282 	}
283 
284 	return buffer;
285 }
286 
287 static void stf_buf_add_pending(struct stf_v_buf *output,
288 				struct stfcamss_buffer *buffer)
289 {
290 	INIT_LIST_HEAD(&buffer->queue);
291 	list_add_tail(&buffer->queue, &output->pending_bufs);
292 }
293 
294 static void stf_buf_update_on_last(struct stf_v_buf *output)
295 {
296 	switch (output->state) {
297 	case STF_OUTPUT_CONTINUOUS:
298 		output->state = STF_OUTPUT_SINGLE;
299 		output->active_buf = !output->active_buf;
300 		break;
301 	case STF_OUTPUT_SINGLE:
302 		output->state = STF_OUTPUT_STOPPING;
303 		break;
304 	default:
305 		break;
306 	}
307 }
308 
309 static void stf_buf_update_on_next(struct stf_v_buf *output)
310 {
311 	switch (output->state) {
312 	case STF_OUTPUT_CONTINUOUS:
313 		output->active_buf = !output->active_buf;
314 		break;
315 	case STF_OUTPUT_SINGLE:
316 	default:
317 		break;
318 	}
319 }
320 
321 static void stf_buf_update_on_new(struct stfcamss_video *video,
322 				  struct stfcamss_buffer *new_buf)
323 {
324 	struct stf_capture *cap = to_stf_capture(video);
325 	struct stf_v_buf *output = &cap->buffers;
326 
327 	switch (output->state) {
328 	case STF_OUTPUT_SINGLE:
329 		stf_buf_add_pending(output, new_buf);
330 		break;
331 	case STF_OUTPUT_IDLE:
332 		if (!output->buf[0]) {
333 			output->buf[0] = new_buf;
334 			stf_init_addrs(video);
335 			output->state = STF_OUTPUT_SINGLE;
336 		} else {
337 			stf_buf_add_pending(output, new_buf);
338 		}
339 		break;
340 	case STF_OUTPUT_STOPPING:
341 		if (output->last_buffer) {
342 			output->buf[output->active_buf] = output->last_buffer;
343 			output->last_buffer = NULL;
344 		}
345 
346 		output->state = STF_OUTPUT_SINGLE;
347 		stf_buf_add_pending(output, new_buf);
348 		break;
349 	case STF_OUTPUT_CONTINUOUS:
350 	default:
351 		stf_buf_add_pending(output, new_buf);
352 		break;
353 	}
354 }
355 
356 static void stf_buf_flush(struct stf_v_buf *output, enum vb2_buffer_state state)
357 {
358 	struct stfcamss_buffer *buf;
359 	struct stfcamss_buffer *t;
360 
361 	list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
362 		vb2_buffer_done(&buf->vb.vb2_buf, state);
363 		list_del(&buf->queue);
364 	}
365 	list_for_each_entry_safe(buf, t, &output->ready_bufs, queue) {
366 		vb2_buffer_done(&buf->vb.vb2_buf, state);
367 		list_del(&buf->queue);
368 	}
369 }
370 
371 static void stf_buf_done(struct stf_v_buf *output)
372 {
373 	struct stfcamss_buffer *ready_buf;
374 	u64 ts = ktime_get_ns();
375 	unsigned long flags;
376 
377 	if (output->state == STF_OUTPUT_OFF ||
378 	    output->state == STF_OUTPUT_RESERVED)
379 		return;
380 
381 	spin_lock_irqsave(&output->lock, flags);
382 
383 	while ((ready_buf = stf_buf_get_ready(output))) {
384 		ready_buf->vb.vb2_buf.timestamp = ts;
385 		ready_buf->vb.sequence = output->sequence++;
386 
387 		vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
388 	}
389 
390 	spin_unlock_irqrestore(&output->lock, flags);
391 }
392 
393 static void stf_change_buffer(struct stf_v_buf *output)
394 {
395 	struct stf_capture *cap = container_of(output, struct stf_capture,
396 					       buffers);
397 	struct stfcamss *stfcamss = cap->video.stfcamss;
398 	struct stfcamss_buffer *ready_buf;
399 	dma_addr_t *new_addr;
400 	unsigned long flags;
401 	u32 active_index;
402 
403 	if (output->state == STF_OUTPUT_OFF ||
404 	    output->state == STF_OUTPUT_STOPPING ||
405 	    output->state == STF_OUTPUT_RESERVED ||
406 	    output->state == STF_OUTPUT_IDLE)
407 		return;
408 
409 	spin_lock_irqsave(&output->lock, flags);
410 
411 	active_index = output->active_buf;
412 
413 	ready_buf = output->buf[active_index];
414 	if (!ready_buf) {
415 		dev_dbg(stfcamss->dev, "missing ready buf %d %d.\n",
416 			active_index, output->state);
417 		active_index = !active_index;
418 		ready_buf = output->buf[active_index];
419 		if (!ready_buf) {
420 			dev_dbg(stfcamss->dev,
421 				"missing ready buf2 %d %d.\n",
422 				active_index, output->state);
423 			goto out_unlock;
424 		}
425 	}
426 
427 	/* Get next buffer */
428 	output->buf[active_index] = stf_buf_get_pending(output);
429 	if (!output->buf[active_index]) {
430 		new_addr = ready_buf->addr;
431 		stf_buf_update_on_last(output);
432 	} else {
433 		new_addr = output->buf[active_index]->addr;
434 		stf_buf_update_on_next(output);
435 	}
436 
437 	if (output->state == STF_OUTPUT_STOPPING) {
438 		output->last_buffer = ready_buf;
439 	} else {
440 		if (cap->type == STF_CAPTURE_RAW)
441 			stf_set_raw_addr(stfcamss, new_addr[0]);
442 		else if (cap->type == STF_CAPTURE_YUV)
443 			stf_set_yuv_addr(stfcamss, new_addr[0], new_addr[1]);
444 
445 		stf_buf_add_ready(output, ready_buf);
446 	}
447 
448 out_unlock:
449 	spin_unlock_irqrestore(&output->lock, flags);
450 }
451 
452 irqreturn_t stf_wr_irq_handler(int irq, void *priv)
453 {
454 	struct stfcamss *stfcamss = priv;
455 	struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_RAW];
456 
457 	if (atomic_dec_if_positive(&cap->buffers.frame_skip) < 0) {
458 		stf_change_buffer(&cap->buffers);
459 		stf_buf_done(&cap->buffers);
460 	}
461 
462 	stf_syscon_reg_set_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN);
463 	stf_syscon_reg_clear_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN);
464 
465 	return IRQ_HANDLED;
466 }
467 
468 irqreturn_t stf_isp_irq_handler(int irq, void *priv)
469 {
470 	struct stfcamss *stfcamss = priv;
471 	struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_YUV];
472 	u32 status;
473 
474 	status = stf_isp_reg_read(stfcamss, ISP_REG_ISP_CTRL_0);
475 	if (status & ISPC_ISP) {
476 		if (status & ISPC_ENUO)
477 			stf_buf_done(&cap->buffers);
478 
479 		stf_isp_reg_write(stfcamss, ISP_REG_ISP_CTRL_0,
480 				  (status & ~ISPC_INT_ALL_MASK) |
481 				  ISPC_ISP | ISPC_CSI | ISPC_SC);
482 	}
483 
484 	return IRQ_HANDLED;
485 }
486 
487 irqreturn_t stf_line_irq_handler(int irq, void *priv)
488 {
489 	struct stfcamss *stfcamss = priv;
490 	struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_YUV];
491 	u32 status;
492 
493 	status = stf_isp_reg_read(stfcamss, ISP_REG_ISP_CTRL_0);
494 	if (status & ISPC_LINE) {
495 		if (atomic_dec_if_positive(&cap->buffers.frame_skip) < 0) {
496 			if ((status & ISPC_ENUO))
497 				stf_change_buffer(&cap->buffers);
498 		}
499 
500 		stf_isp_reg_set_bit(stfcamss, ISP_REG_CSIINTS,
501 				    CSI_INTS_MASK, CSI_INTS(0x3));
502 		stf_isp_reg_set_bit(stfcamss, ISP_REG_IESHD,
503 				    SHAD_UP_M | SHAD_UP_EN, 0x3);
504 
505 		stf_isp_reg_write(stfcamss, ISP_REG_ISP_CTRL_0,
506 				  (status & ~ISPC_INT_ALL_MASK) | ISPC_LINE);
507 	}
508 
509 	return IRQ_HANDLED;
510 }
511 
512 static int stf_queue_buffer(struct stfcamss_video *video,
513 			    struct stfcamss_buffer *buf)
514 {
515 	struct stf_capture *cap = to_stf_capture(video);
516 	struct stf_v_buf *v_bufs = &cap->buffers;
517 	unsigned long flags;
518 
519 	spin_lock_irqsave(&v_bufs->lock, flags);
520 	stf_buf_update_on_new(video, buf);
521 	spin_unlock_irqrestore(&v_bufs->lock, flags);
522 
523 	return 0;
524 }
525 
526 static int stf_flush_buffers(struct stfcamss_video *video,
527 			     enum vb2_buffer_state state)
528 {
529 	struct stf_capture *cap = to_stf_capture(video);
530 	struct stf_v_buf *v_bufs = &cap->buffers;
531 	unsigned long flags;
532 	unsigned int i;
533 
534 	spin_lock_irqsave(&v_bufs->lock, flags);
535 
536 	stf_buf_flush(v_bufs, state);
537 
538 	for (i = 0; i < ARRAY_SIZE(v_bufs->buf); i++) {
539 		if (v_bufs->buf[i])
540 			vb2_buffer_done(&v_bufs->buf[i]->vb.vb2_buf, state);
541 
542 		v_bufs->buf[i] = NULL;
543 	}
544 
545 	if (v_bufs->last_buffer) {
546 		vb2_buffer_done(&v_bufs->last_buffer->vb.vb2_buf, state);
547 		v_bufs->last_buffer = NULL;
548 	}
549 
550 	spin_unlock_irqrestore(&v_bufs->lock, flags);
551 	return 0;
552 }
553 
554 static const struct stfcamss_video_ops stf_capture_ops = {
555 	.queue_buffer = stf_queue_buffer,
556 	.flush_buffers = stf_flush_buffers,
557 	.start_streaming = stf_capture_start,
558 	.stop_streaming = stf_capture_stop,
559 };
560 
561 static void stf_capture_unregister_one(struct stf_capture *cap)
562 {
563 	if (!video_is_registered(&cap->video.vdev))
564 		return;
565 
566 	media_entity_cleanup(&cap->video.vdev.entity);
567 	vb2_video_unregister_device(&cap->video.vdev);
568 }
569 
570 void stf_capture_unregister(struct stfcamss *stfcamss)
571 {
572 	struct stf_capture *cap_raw = &stfcamss->captures[STF_CAPTURE_RAW];
573 	struct stf_capture *cap_yuv = &stfcamss->captures[STF_CAPTURE_YUV];
574 
575 	stf_capture_unregister_one(cap_raw);
576 	stf_capture_unregister_one(cap_yuv);
577 }
578 
579 int stf_capture_register(struct stfcamss *stfcamss,
580 			 struct v4l2_device *v4l2_dev)
581 {
582 	unsigned int i;
583 	int ret;
584 
585 	for (i = 0; i < ARRAY_SIZE(stfcamss->captures); i++) {
586 		struct stf_capture *capture = &stfcamss->captures[i];
587 
588 		capture->type = i;
589 		capture->video.ops = &stf_capture_ops;
590 		stf_capture_init(stfcamss, capture);
591 
592 		ret = stf_video_register(&capture->video, v4l2_dev,
593 					 stf_cap_names[i]);
594 		if (ret < 0) {
595 			dev_err(stfcamss->dev,
596 				"Failed to register video node: %d\n", ret);
597 			stf_capture_unregister(stfcamss);
598 			return ret;
599 		}
600 	}
601 
602 	return 0;
603 }
604