kernel_5.10 no rt

hc

2023-12-11 6778948f9de86c3cfaf36725a7c87dcff9ba247f

 kernel/drivers/media/platform/rockchip/cif/cif-tools.c
..
..
@@ -300,8 +300,8 @@
300
300
 	input_rect.height = RKCIF_DEFAULT_HEIGHT;
301
301
 
302
302
 	if (terminal_sensor && terminal_sensor->sd)
303
-		get_input_fmt(terminal_sensor->sd,
304
-			      &input_rect, 0, &csi_info);
303
+		rkcif_get_input_fmt(dev,
304
+				    &input_rect, 0, &csi_info);
305
305
 
306
306
 	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
307
307
 	s->width = input_rect.width;
..
..
@@ -589,11 +589,8 @@
589
589
 	return vb2_queue_init(q);
590
590
 }
591
591
 
592
-static void rkcif_tools_work(struct work_struct *work)
592
+static void rkcif_tools_buf_done(struct rkcif_tools_vdev *tools_vdev)
593
593
 {
594
-	struct rkcif_tools_vdev *tools_vdev = container_of(work,
595
-						struct rkcif_tools_vdev,
596
-						work);
597
594
 	struct rkcif_stream *stream = tools_vdev->stream;
598
595
 	struct rkcif_tools_buffer *tools_buf;
599
596
 	const struct cif_output_fmt *fmt = tools_vdev->tools_out_fmt;
..
..
@@ -700,6 +697,99 @@
700
697
 	spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
701
698
 }
702
699
 
700
+static void rkcif_tools_buf_done_rdbk(struct rkcif_tools_vdev *tools_vdev)
701
+{
702
+	struct rkcif_stream *stream = tools_vdev->stream;
703
+	struct rkcif_device *dev = stream->cifdev;
704
+	const struct cif_output_fmt *fmt = tools_vdev->tools_out_fmt;
705
+	struct rkcif_rx_buffer *buf = NULL;
706
+	int i = 0;
707
+	unsigned long flags;
708
+
709
+retry_done_rdbk_buf:
710
+	spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
711
+	if (!list_empty(&tools_vdev->buf_done_head)) {
712
+		buf = list_first_entry(&tools_vdev->buf_done_head,
713
+				       struct rkcif_rx_buffer, list);
714
+		if (buf)
715
+			list_del(&buf->list);
716
+	}
717
+	spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
718
+	if (!buf) {
719
+		v4l2_err(&dev->v4l2_dev, "stream[%d] tools fail to get buf form list\n",
720
+			 stream->id);
721
+		return;
722
+	}
723
+
724
+	if (tools_vdev->stopping) {
725
+		rkcif_tools_stop(tools_vdev);
726
+		tools_vdev->stopping = false;
727
+		spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
728
+		while (!list_empty(&tools_vdev->buf_done_head)) {
729
+			buf = list_first_entry(&tools_vdev->buf_done_head,
730
+					       struct rkcif_rx_buffer, list);
731
+			if (buf)
732
+				list_del(&buf->list);
733
+		}
734
+		spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
735
+		wake_up(&tools_vdev->wq_stopped);
736
+		return;
737
+	}
738
+
739
+	if (!list_empty(&tools_vdev->buf_head)) {
740
+		tools_vdev->curr_buf = list_first_entry(&tools_vdev->buf_head,
741
+						    struct rkcif_buffer, queue);
742
+		if (!tools_vdev->curr_buf || tools_vdev->state != RKCIF_STATE_STREAMING) {
743
+			spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
744
+			if (!list_empty(&tools_vdev->buf_done_head)) {
745
+				spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
746
+				goto retry_done_rdbk_buf;
747
+			}
748
+			spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
749
+			return;
750
+		}
751
+		list_del(&tools_vdev->curr_buf->queue);
752
+		/* Dequeue a filled buffer */
753
+		for (i = 0; i < fmt->mplanes; i++) {
754
+			u32 payload_size = tools_vdev->pixm.plane_fmt[i].sizeimage;
755
+			void *src = buf->dummy.vaddr;
756
+			void *dst = vb2_plane_vaddr(&tools_vdev->curr_buf->vb.vb2_buf, i);
757
+
758
+			if (!src || !dst)
759
+				break;
760
+			dma_sync_single_for_device(dev->dev,
761
+						   buf->dummy.dma_addr,
762
+						   buf->dummy.size,
763
+						   DMA_FROM_DEVICE);
764
+			vb2_set_plane_payload(&tools_vdev->curr_buf->vb.vb2_buf, i,
765
+					      payload_size);
766
+			memcpy(dst, src, payload_size);
767
+		}
768
+		tools_vdev->curr_buf->vb.sequence = buf->dbufs.sequence;
769
+		tools_vdev->curr_buf->vb.vb2_buf.timestamp = buf->dbufs.timestamp;
770
+		vb2_buffer_done(&tools_vdev->curr_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
771
+		tools_vdev->curr_buf = NULL;
772
+	}
773
+
774
+	spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
775
+	if (!list_empty(&tools_vdev->buf_done_head)) {
776
+		spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
777
+		goto retry_done_rdbk_buf;
778
+	}
779
+	spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
780
+}
781
+
782
+static void rkcif_tools_work(struct work_struct *work)
783
+{
784
+	struct rkcif_tools_vdev *tools_vdev = container_of(work,
785
+						struct rkcif_tools_vdev,
786
+						work);
787
+	if (tools_vdev->stream->dma_en & RKCIF_DMAEN_BY_VICAP)
788
+		rkcif_tools_buf_done(tools_vdev);
789
+	else if (tools_vdev->stream->dma_en & RKCIF_DMAEN_BY_ISP)
790
+		rkcif_tools_buf_done_rdbk(tools_vdev);
791
+}
792
+
703
793
 void rkcif_init_tools_vdev(struct rkcif_device *cif_dev, u32 ch)
704
794
 {
705
795
 	struct rkcif_tools_vdev *tools_vdev = &cif_dev->tools_vdev[ch];