stf-capture.c

  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
 12static const char * const stf_cap_names[] = {
 13	"capture_raw",
 14	"capture_yuv",
 15};
 16
 17static 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
 48static 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
 58static inline struct stf_capture *to_stf_capture(struct stfcamss_video *video)
 59{
 60	return container_of(video, struct stf_capture, video);
 61}
 62
 63static 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
 69static 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
 76static 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
 96static 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
110static 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
135static 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
150static 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
158static 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
166static 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
176static 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		const struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
184
185		val = stf_syscon_reg_read(stfcamss, VIN_CHANNEL_SEL_EN);
186		val &= ~U0_VIN_CHANNEL_SEL_MASK;
187		val |= CHANNEL(0);
188		stf_syscon_reg_write(stfcamss, VIN_CHANNEL_SEL_EN, val);
189
190		val = stf_syscon_reg_read(stfcamss, VIN_INRT_PIX_CFG);
191		val &= ~U0_VIN_PIX_CT_MASK;
192		val |= PIX_CT(1);
193
194		val &= ~U0_VIN_PIXEL_HEIGH_BIT_SEL_MAKS;
195		val |= PIXEL_HEIGH_BIT_SEL(0);
196
197		val &= ~U0_VIN_PIX_CNT_END_MASK;
198		val |= PIX_CNT_END(pix->width / 4 - 1);
199
200		stf_syscon_reg_write(stfcamss, VIN_INRT_PIX_CFG, val);
201	} else if (cap->type == STF_CAPTURE_YUV) {
202		val = stf_syscon_reg_read(stfcamss, VIN_CFG_REG);
203		val &= ~U0_VIN_MIPI_BYTE_EN_ISP0_MASK;
204		val |= U0_VIN_MIPI_BYTE_EN_ISP0(0);
205
206		val &= ~U0_VIN_MIPI_CHANNEL_SEL0_MASK;
207		val |= U0_VIN_MIPI_CHANNEL_SEL0(0);
208
209		val &= ~U0_VIN_PIX_NUM_MASK;
210		val |= U0_VIN_PIX_NUM(0);
211
212		val &= ~U0_VIN_P_I_MIPI_HAEDER_EN0_MASK;
213		val |= U0_VIN_P_I_MIPI_HAEDER_EN0(1);
214
215		stf_syscon_reg_write(stfcamss, VIN_CFG_REG, val);
216	}
217}
218
219static void stf_capture_start(struct stfcamss_video *video)
220{
221	struct stf_capture *cap = to_stf_capture(video);
222
223	stf_channel_set(video);
224	if (cap->type == STF_CAPTURE_RAW) {
225		stf_wr_irq_enable(video);
226		stf_wr_data_en(video);
227	}
228
229	stf_cap_s_cfg(video);
230}
231
232static void stf_capture_stop(struct stfcamss_video *video)
233{
234	struct stf_capture *cap = to_stf_capture(video);
235
236	if (cap->type == STF_CAPTURE_RAW)
237		stf_wr_irq_disable(video);
238
239	stf_cap_s_cleanup(video);
240}
241
242static void stf_capture_init(struct stfcamss *stfcamss, struct stf_capture *cap)
243{
244	cap->buffers.state = STF_OUTPUT_OFF;
245	cap->buffers.buf[0] = NULL;
246	cap->buffers.buf[1] = NULL;
247	cap->buffers.active_buf = 0;
248	atomic_set(&cap->buffers.frame_skip, 4);
249	INIT_LIST_HEAD(&cap->buffers.pending_bufs);
250	INIT_LIST_HEAD(&cap->buffers.ready_bufs);
251	spin_lock_init(&cap->buffers.lock);
252
253	cap->video.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
254	cap->video.stfcamss = stfcamss;
255	cap->video.bpl_alignment = 16 * 8;
256
257	if (cap->type == STF_CAPTURE_RAW) {
258		cap->video.formats = stf_wr_fmts;
259		cap->video.nformats = ARRAY_SIZE(stf_wr_fmts);
260		cap->video.bpl_alignment = 8;
261	} else if (cap->type == STF_CAPTURE_YUV) {
262		cap->video.formats = stf_isp_fmts;
263		cap->video.nformats = ARRAY_SIZE(stf_isp_fmts);
264		cap->video.bpl_alignment = 1;
265	}
266}
267
268static void stf_buf_add_ready(struct stf_v_buf *output,
269			      struct stfcamss_buffer *buffer)
270{
271	INIT_LIST_HEAD(&buffer->queue);
272	list_add_tail(&buffer->queue, &output->ready_bufs);
273}
274
275static struct stfcamss_buffer *stf_buf_get_ready(struct stf_v_buf *output)
276{
277	struct stfcamss_buffer *buffer = NULL;
278
279	if (!list_empty(&output->ready_bufs)) {
280		buffer = list_first_entry(&output->ready_bufs,
281					  struct stfcamss_buffer,
282					  queue);
283		list_del(&buffer->queue);
284	}
285
286	return buffer;
287}
288
289static void stf_buf_add_pending(struct stf_v_buf *output,
290				struct stfcamss_buffer *buffer)
291{
292	INIT_LIST_HEAD(&buffer->queue);
293	list_add_tail(&buffer->queue, &output->pending_bufs);
294}
295
296static void stf_buf_update_on_last(struct stf_v_buf *output)
297{
298	switch (output->state) {
299	case STF_OUTPUT_CONTINUOUS:
300		output->state = STF_OUTPUT_SINGLE;
301		output->active_buf = !output->active_buf;
302		break;
303	case STF_OUTPUT_SINGLE:
304		output->state = STF_OUTPUT_STOPPING;
305		break;
306	default:
307		break;
308	}
309}
310
311static void stf_buf_update_on_next(struct stf_v_buf *output)
312{
313	switch (output->state) {
314	case STF_OUTPUT_CONTINUOUS:
315		output->active_buf = !output->active_buf;
316		break;
317	case STF_OUTPUT_SINGLE:
318	default:
319		break;
320	}
321}
322
323static void stf_buf_update_on_new(struct stfcamss_video *video,
324				  struct stfcamss_buffer *new_buf)
325{
326	struct stf_capture *cap = to_stf_capture(video);
327	struct stf_v_buf *output = &cap->buffers;
328
329	switch (output->state) {
330	case STF_OUTPUT_SINGLE:
331		stf_buf_add_pending(output, new_buf);
332		break;
333	case STF_OUTPUT_IDLE:
334		if (!output->buf[0]) {
335			output->buf[0] = new_buf;
336			stf_init_addrs(video);
337			output->state = STF_OUTPUT_SINGLE;
338		} else {
339			stf_buf_add_pending(output, new_buf);
340		}
341		break;
342	case STF_OUTPUT_STOPPING:
343		if (output->last_buffer) {
344			output->buf[output->active_buf] = output->last_buffer;
345			output->last_buffer = NULL;
346		}
347
348		output->state = STF_OUTPUT_SINGLE;
349		stf_buf_add_pending(output, new_buf);
350		break;
351	case STF_OUTPUT_CONTINUOUS:
352	default:
353		stf_buf_add_pending(output, new_buf);
354		break;
355	}
356}
357
358static void stf_buf_flush(struct stf_v_buf *output, enum vb2_buffer_state state)
359{
360	struct stfcamss_buffer *buf;
361	struct stfcamss_buffer *t;
362
363	list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
364		vb2_buffer_done(&buf->vb.vb2_buf, state);
365		list_del(&buf->queue);
366	}
367	list_for_each_entry_safe(buf, t, &output->ready_bufs, queue) {
368		vb2_buffer_done(&buf->vb.vb2_buf, state);
369		list_del(&buf->queue);
370	}
371}
372
373static void stf_buf_done(struct stf_v_buf *output)
374{
375	struct stfcamss_buffer *ready_buf;
376	u64 ts = ktime_get_ns();
377	unsigned long flags;
378
379	if (output->state == STF_OUTPUT_OFF ||
380	    output->state == STF_OUTPUT_RESERVED)
381		return;
382
383	spin_lock_irqsave(&output->lock, flags);
384
385	while ((ready_buf = stf_buf_get_ready(output))) {
386		ready_buf->vb.vb2_buf.timestamp = ts;
387		ready_buf->vb.sequence = output->sequence++;
388
389		vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
390	}
391
392	spin_unlock_irqrestore(&output->lock, flags);
393}
394
395static void stf_change_buffer(struct stf_v_buf *output)
396{
397	struct stf_capture *cap = container_of(output, struct stf_capture,
398					       buffers);
399	struct stfcamss *stfcamss = cap->video.stfcamss;
400	struct stfcamss_buffer *ready_buf;
401	dma_addr_t *new_addr;
402	unsigned long flags;
403	u32 active_index;
404
405	if (output->state == STF_OUTPUT_OFF ||
406	    output->state == STF_OUTPUT_STOPPING ||
407	    output->state == STF_OUTPUT_RESERVED ||
408	    output->state == STF_OUTPUT_IDLE)
409		return;
410
411	spin_lock_irqsave(&output->lock, flags);
412
413	active_index = output->active_buf;
414
415	ready_buf = output->buf[active_index];
416	if (!ready_buf) {
417		dev_dbg(stfcamss->dev, "missing ready buf %d %d.\n",
418			active_index, output->state);
419		active_index = !active_index;
420		ready_buf = output->buf[active_index];
421		if (!ready_buf) {
422			dev_dbg(stfcamss->dev,
423				"missing ready buf2 %d %d.\n",
424				active_index, output->state);
425			goto out_unlock;
426		}
427	}
428
429	/* Get next buffer */
430	output->buf[active_index] = stf_buf_get_pending(output);
431	if (!output->buf[active_index]) {
432		new_addr = ready_buf->addr;
433		stf_buf_update_on_last(output);
434	} else {
435		new_addr = output->buf[active_index]->addr;
436		stf_buf_update_on_next(output);
437	}
438
439	if (output->state == STF_OUTPUT_STOPPING) {
440		output->last_buffer = ready_buf;
441	} else {
442		if (cap->type == STF_CAPTURE_RAW)
443			stf_set_raw_addr(stfcamss, new_addr[0]);
444		else if (cap->type == STF_CAPTURE_YUV)
445			stf_set_yuv_addr(stfcamss, new_addr[0], new_addr[1]);
446
447		stf_buf_add_ready(output, ready_buf);
448	}
449
450out_unlock:
451	spin_unlock_irqrestore(&output->lock, flags);
452}
453
454irqreturn_t stf_wr_irq_handler(int irq, void *priv)
455{
456	struct stfcamss *stfcamss = priv;
457	struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_RAW];
458
459	if (atomic_dec_if_positive(&cap->buffers.frame_skip) < 0) {
460		stf_change_buffer(&cap->buffers);
461		stf_buf_done(&cap->buffers);
462	}
463
464	stf_syscon_reg_set_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN);
465	stf_syscon_reg_clear_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN);
466
467	return IRQ_HANDLED;
468}
469
470irqreturn_t stf_isp_irq_handler(int irq, void *priv)
471{
472	struct stfcamss *stfcamss = priv;
473	struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_YUV];
474	u32 status;
475
476	status = stf_isp_reg_read(stfcamss, ISP_REG_ISP_CTRL_0);
477	if (status & ISPC_ISP) {
478		if (status & ISPC_ENUO)
479			stf_buf_done(&cap->buffers);
480
481		stf_isp_reg_write(stfcamss, ISP_REG_ISP_CTRL_0,
482				  (status & ~ISPC_INT_ALL_MASK) |
483				  ISPC_ISP | ISPC_CSI | ISPC_SC);
484	}
485
486	return IRQ_HANDLED;
487}
488
489irqreturn_t stf_line_irq_handler(int irq, void *priv)
490{
491	struct stfcamss *stfcamss = priv;
492	struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_YUV];
493	u32 status;
494
495	status = stf_isp_reg_read(stfcamss, ISP_REG_ISP_CTRL_0);
496	if (status & ISPC_LINE) {
497		if (atomic_dec_if_positive(&cap->buffers.frame_skip) < 0) {
498			if ((status & ISPC_ENUO))
499				stf_change_buffer(&cap->buffers);
500		}
501
502		stf_isp_reg_set_bit(stfcamss, ISP_REG_CSIINTS,
503				    CSI_INTS_MASK, CSI_INTS(0x3));
504		stf_isp_reg_set_bit(stfcamss, ISP_REG_IESHD,
505				    SHAD_UP_M | SHAD_UP_EN, 0x3);
506
507		stf_isp_reg_write(stfcamss, ISP_REG_ISP_CTRL_0,
508				  (status & ~ISPC_INT_ALL_MASK) | ISPC_LINE);
509	}
510
511	return IRQ_HANDLED;
512}
513
514static int stf_queue_buffer(struct stfcamss_video *video,
515			    struct stfcamss_buffer *buf)
516{
517	struct stf_capture *cap = to_stf_capture(video);
518	struct stf_v_buf *v_bufs = &cap->buffers;
519	unsigned long flags;
520
521	spin_lock_irqsave(&v_bufs->lock, flags);
522	stf_buf_update_on_new(video, buf);
523	spin_unlock_irqrestore(&v_bufs->lock, flags);
524
525	return 0;
526}
527
528static int stf_flush_buffers(struct stfcamss_video *video,
529			     enum vb2_buffer_state state)
530{
531	struct stf_capture *cap = to_stf_capture(video);
532	struct stf_v_buf *v_bufs = &cap->buffers;
533	unsigned long flags;
534	unsigned int i;
535
536	spin_lock_irqsave(&v_bufs->lock, flags);
537
538	stf_buf_flush(v_bufs, state);
539
540	for (i = 0; i < ARRAY_SIZE(v_bufs->buf); i++) {
541		if (v_bufs->buf[i])
542			vb2_buffer_done(&v_bufs->buf[i]->vb.vb2_buf, state);
543
544		v_bufs->buf[i] = NULL;
545	}
546
547	if (v_bufs->last_buffer) {
548		vb2_buffer_done(&v_bufs->last_buffer->vb.vb2_buf, state);
549		v_bufs->last_buffer = NULL;
550	}
551
552	spin_unlock_irqrestore(&v_bufs->lock, flags);
553	return 0;
554}
555
556static const struct stfcamss_video_ops stf_capture_ops = {
557	.queue_buffer = stf_queue_buffer,
558	.flush_buffers = stf_flush_buffers,
559	.start_streaming = stf_capture_start,
560	.stop_streaming = stf_capture_stop,
561};
562
563static void stf_capture_unregister_one(struct stf_capture *cap)
564{
565	if (!video_is_registered(&cap->video.vdev))
566		return;
567
568	media_entity_cleanup(&cap->video.vdev.entity);
569	vb2_video_unregister_device(&cap->video.vdev);
570}
571
572void stf_capture_unregister(struct stfcamss *stfcamss)
573{
574	struct stf_capture *cap_raw = &stfcamss->captures[STF_CAPTURE_RAW];
575	struct stf_capture *cap_yuv = &stfcamss->captures[STF_CAPTURE_YUV];
576
577	stf_capture_unregister_one(cap_raw);
578	stf_capture_unregister_one(cap_yuv);
579}
580
581int stf_capture_register(struct stfcamss *stfcamss,
582			 struct v4l2_device *v4l2_dev)
583{
584	unsigned int i;
585	int ret;
586
587	for (i = 0; i < ARRAY_SIZE(stfcamss->captures); i++) {
588		struct stf_capture *capture = &stfcamss->captures[i];
589
590		capture->type = i;
591		capture->video.ops = &stf_capture_ops;
592		stf_capture_init(stfcamss, capture);
593
594		ret = stf_video_register(&capture->video, v4l2_dev,
595					 stf_cap_names[i]);
596		if (ret < 0) {
597			dev_err(stfcamss->dev,
598				"Failed to register video node: %d\n", ret);
599			stf_capture_unregister(stfcamss);
600			return ret;
601		}
602	}
603
604	return 0;
605}