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
to_stf_capture(struct stfcamss_video * video)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
stf_set_raw_addr(struct stfcamss * stfcamss,dma_addr_t addr)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
stf_set_yuv_addr(struct stfcamss * stfcamss,dma_addr_t y_addr,dma_addr_t uv_addr)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
stf_init_addrs(struct stfcamss_video * video)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
stf_buf_get_pending(struct stf_v_buf * output)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
stf_cap_s_cfg(struct stfcamss_video * video)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
stf_cap_s_cleanup(struct stfcamss_video * video)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
stf_wr_data_en(struct stfcamss_video * video)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
stf_wr_irq_enable(struct stfcamss_video * video)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
stf_wr_irq_disable(struct stfcamss_video * video)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
stf_channel_set(struct stfcamss_video * video)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 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
stf_capture_start(struct stfcamss_video * video)219 static 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
stf_capture_stop(struct stfcamss_video * video)232 static 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
stf_capture_init(struct stfcamss * stfcamss,struct stf_capture * cap)242 static 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
stf_buf_add_ready(struct stf_v_buf * output,struct stfcamss_buffer * buffer)268 static 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
stf_buf_get_ready(struct stf_v_buf * output)275 static 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
stf_buf_add_pending(struct stf_v_buf * output,struct stfcamss_buffer * buffer)289 static 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
stf_buf_update_on_last(struct stf_v_buf * output)296 static 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
stf_buf_update_on_next(struct stf_v_buf * output)311 static 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
stf_buf_update_on_new(struct stfcamss_video * video,struct stfcamss_buffer * new_buf)323 static 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
stf_buf_flush(struct stf_v_buf * output,enum vb2_buffer_state state)358 static 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
stf_buf_done(struct stf_v_buf * output)373 static 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
stf_change_buffer(struct stf_v_buf * output)395 static 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
450 out_unlock:
451 spin_unlock_irqrestore(&output->lock, flags);
452 }
453
stf_wr_irq_handler(int irq,void * priv)454 irqreturn_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
stf_isp_irq_handler(int irq,void * priv)470 irqreturn_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
stf_line_irq_handler(int irq,void * priv)489 irqreturn_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
stf_queue_buffer(struct stfcamss_video * video,struct stfcamss_buffer * buf)514 static 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
stf_flush_buffers(struct stfcamss_video * video,enum vb2_buffer_state state)528 static 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
556 static 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
stf_capture_unregister_one(struct stf_capture * cap)563 static 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
stf_capture_unregister(struct stfcamss * stfcamss)572 void 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
stf_capture_register(struct stfcamss * stfcamss,struct v4l2_device * v4l2_dev)581 int 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 }
606