229 static int cio2_fbpt_init(struct cio2_device *cio2, struct cio2_queue *q)  in cio2_fbpt_init()  argument
233 	q->fbpt = dma_alloc_coherent(dev, CIO2_FBPT_SIZE, &q->fbpt_bus_addr,  in cio2_fbpt_init()
235 	if (!q->fbpt)  in cio2_fbpt_init()
241 static void cio2_fbpt_exit(struct cio2_queue *q, struct device *dev)  in cio2_fbpt_exit()  argument
243 	dma_free_coherent(dev, CIO2_FBPT_SIZE, q->fbpt, q->fbpt_bus_addr);  in cio2_fbpt_exit()
306 static int cio2_csi2_calc_timing(struct cio2_device *cio2, struct cio2_queue *q,  in cio2_csi2_calc_timing()  argument
313 	if (!q->sensor)  in cio2_csi2_calc_timing()
316 	freq = v4l2_get_link_freq(q->sensor->ctrl_handler, bpp, lanes * 2);  in cio2_csi2_calc_timing()
347 static int cio2_hw_init(struct cio2_device *cio2, struct cio2_queue *q)  in cio2_hw_init()  argument
357 	u8 lanes, csi2bus = q->csi2.port;  in cio2_hw_init()
362 	fmt = cio2_find_format(NULL, &q->subdev_fmt.code);  in cio2_hw_init()
366 	lanes = q->csi2.lanes;  in cio2_hw_init()
368 	r = cio2_csi2_calc_timing(cio2, q, &timing, fmt->bpp, lanes);  in cio2_hw_init()
372 	writel(timing.clk_termen, q->csi_rx_base +  in cio2_hw_init()
374 	writel(timing.clk_settle, q->csi_rx_base +  in cio2_hw_init()
378 		writel(timing.dat_termen, q->csi_rx_base +  in cio2_hw_init()
380 		writel(timing.dat_settle, q->csi_rx_base +  in cio2_hw_init()
402 	       q->csi_rx_base + CIO2_REG_CSIRX_STATUS_DLANE_HS);  in cio2_hw_init()
404 	       q->csi_rx_base + CIO2_REG_CSIRX_STATUS_DLANE_LP);  in cio2_hw_init()
411 		writel(1, q->csi_rx_base + CIO2_REG_MIPIBE_SP_LUT_ENTRY(i));  in cio2_hw_init()
416 		       q->csi_rx_base + CIO2_REG_MIPIBE_LP_LUT_ENTRY(i));  in cio2_hw_init()
418 	       q->csi_rx_base + CIO2_REG_MIPIBE_GLOBAL_LUT_DISREGARD);  in cio2_hw_init()
421 	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_MASK);  in cio2_hw_init()
422 	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_ENABLE);  in cio2_hw_init()
423 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_EDGE);  in cio2_hw_init()
424 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_LEVEL_NOT_PULSE);  in cio2_hw_init()
437 	       q->csi_rx_base + CIO2_REG_MIPIBE_LP_LUT_ENTRY(ENTRY));  in cio2_hw_init()
438 	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_COMP_FORMAT(sensor_vc));  in cio2_hw_init()
439 	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_FORCE_RAW8);  in cio2_hw_init()
442 	writel(lanes, q->csi_rx_base + CIO2_REG_CSIRX_NOF_ENABLED_LANES);  in cio2_hw_init()
475 	writel(PFN_DOWN(q->fbpt_bus_addr), base + CIO2_REG_CDMABA(CIO2_DMA_CHAN));  in cio2_hw_init()
494 	       base + CIO2_REG_PXM_FRF_CFG(q->csi2.port));  in cio2_hw_init()
497 	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_CLEAR);  in cio2_hw_init()
503 	writel(1, q->csi_rx_base + CIO2_REG_MIPIBE_ENABLE);  in cio2_hw_init()
504 	writel(1, q->csi_rx_base + CIO2_REG_CSIRX_ENABLE);  in cio2_hw_init()
509 static void cio2_hw_exit(struct cio2_device *cio2, struct cio2_queue *q)  in cio2_hw_exit()  argument
518 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_MASK);  in cio2_hw_exit()
519 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_ENABLE);  in cio2_hw_exit()
520 	writel(0, q->csi_rx_base + CIO2_REG_CSIRX_ENABLE);  in cio2_hw_exit()
521 	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_ENABLE);  in cio2_hw_exit()
542 	struct cio2_queue *q = cio2->cur_queue;  in cio2_buffer_done()  local
551 	entry = &q->fbpt[q->bufs_first * CIO2_MAX_LOPS];  in cio2_buffer_done()
562 		b = q->bufs[q->bufs_first];  in cio2_buffer_done()
568 			q->bufs[q->bufs_first] = NULL;  in cio2_buffer_done()
569 			atomic_dec(&q->bufs_queued);  in cio2_buffer_done()
574 			b->vbb.sequence = atomic_read(&q->frame_sequence);  in cio2_buffer_done()
581 		atomic_inc(&q->frame_sequence);  in cio2_buffer_done()
583 		q->bufs_first = (q->bufs_first + 1) % CIO2_MAX_BUFFERS;  in cio2_buffer_done()
584 		entry = &q->fbpt[q->bufs_first * CIO2_MAX_LOPS];  in cio2_buffer_done()
588 static void cio2_queue_event_sof(struct cio2_device *cio2, struct cio2_queue *q)  in cio2_queue_event_sof()  argument
597 		.u.frame_sync.frame_sequence = atomic_read(&q->frame_sequence),  in cio2_queue_event_sof()
600 	v4l2_event_queue(q->subdev.devnode, &event);  in cio2_queue_event_sof()
781 static void cio2_vb2_return_all_buffers(struct cio2_queue *q,  in cio2_vb2_return_all_buffers()  argument
787 		if (q->bufs[i]) {  in cio2_vb2_return_all_buffers()
788 			atomic_dec(&q->bufs_queued);  in cio2_vb2_return_all_buffers()
789 			vb2_buffer_done(&q->bufs[i]->vbb.vb2_buf,  in cio2_vb2_return_all_buffers()
791 			q->bufs[i] = NULL;  in cio2_vb2_return_all_buffers()
804 	struct cio2_queue *q = vb2q_to_cio2_queue(vq);  in cio2_vb2_queue_setup()  local
807 	if (*num_planes && *num_planes < q->format.num_planes)  in cio2_vb2_queue_setup()
810 	for (i = 0; i < q->format.num_planes; ++i) {  in cio2_vb2_queue_setup()
811 		if (*num_planes && sizes[i] < q->format.plane_fmt[i].sizeimage)  in cio2_vb2_queue_setup()
813 		sizes[i] = q->format.plane_fmt[i].sizeimage;  in cio2_vb2_queue_setup()
817 	*num_planes = q->format.num_planes;  in cio2_vb2_queue_setup()
822 		q->bufs[i] = NULL;  in cio2_vb2_queue_setup()
823 		cio2_fbpt_entry_init_dummy(cio2, &q->fbpt[i * CIO2_MAX_LOPS]);  in cio2_vb2_queue_setup()
825 	atomic_set(&q->bufs_queued, 0);  in cio2_vb2_queue_setup()
826 	q->bufs_first = 0;  in cio2_vb2_queue_setup()
827 	q->bufs_next = 0;  in cio2_vb2_queue_setup()
892 	struct cio2_queue *q =  in cio2_vb2_buf_queue()  local
897 	unsigned int i, j, next = q->bufs_next;  in cio2_vb2_buf_queue()
898 	int bufs_queued = atomic_inc_return(&q->bufs_queued);  in cio2_vb2_buf_queue()
940 		if (!q->bufs[next]) {  in cio2_vb2_buf_queue()
941 			q->bufs[next] = b;  in cio2_vb2_buf_queue()
942 			entry = &q->fbpt[next * CIO2_MAX_LOPS];  in cio2_vb2_buf_queue()
945 			q->bufs_next = (next + 1) % CIO2_MAX_BUFFERS;  in cio2_vb2_buf_queue()
948 					q->format.plane_fmt[j].sizeimage);  in cio2_vb2_buf_queue()
958 	atomic_dec(&q->bufs_queued);  in cio2_vb2_buf_queue()
980 	struct cio2_queue *q = vb2q_to_cio2_queue(vq);  in cio2_vb2_start_streaming()  local
985 	cio2->cur_queue = q;  in cio2_vb2_start_streaming()
986 	atomic_set(&q->frame_sequence, 0);  in cio2_vb2_start_streaming()
994 	r = video_device_pipeline_start(&q->vdev, &q->pipe);  in cio2_vb2_start_streaming()
998 	r = cio2_hw_init(cio2, q);  in cio2_vb2_start_streaming()
1003 	r = v4l2_subdev_call(q->sensor, video, s_stream, 1);  in cio2_vb2_start_streaming()
1012 	cio2_hw_exit(cio2, q);  in cio2_vb2_start_streaming()
1014 	video_device_pipeline_stop(&q->vdev);  in cio2_vb2_start_streaming()
1017 	cio2_vb2_return_all_buffers(q, VB2_BUF_STATE_QUEUED);  in cio2_vb2_start_streaming()
1025 	struct cio2_queue *q = vb2q_to_cio2_queue(vq);  in cio2_vb2_stop_streaming()  local
1029 	if (v4l2_subdev_call(q->sensor, video, s_stream, 0))  in cio2_vb2_stop_streaming()
1032 	cio2_hw_exit(cio2, q);  in cio2_vb2_stop_streaming()
1034 	cio2_vb2_return_all_buffers(q, VB2_BUF_STATE_ERROR);  in cio2_vb2_stop_streaming()
1035 	video_device_pipeline_stop(&q->vdev);  in cio2_vb2_stop_streaming()
1076 	struct cio2_queue *q = file_to_cio2_queue(file);  in cio2_v4l2_g_fmt()  local
1078 	f->fmt.pix_mp = q->format;  in cio2_v4l2_g_fmt()
1116 	struct cio2_queue *q = file_to_cio2_queue(file);  in cio2_v4l2_s_fmt()  local
1119 	q->format = f->fmt.pix_mp;  in cio2_v4l2_s_fmt()
1229 	struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev);  in cio2_subdev_get_fmt()  local
1231 	mutex_lock(&q->subdev_lock);  in cio2_subdev_get_fmt()
1237 		fmt->format = q->subdev_fmt;  in cio2_subdev_get_fmt()
1239 	mutex_unlock(&q->subdev_lock);  in cio2_subdev_get_fmt()
1255 	struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev);  in cio2_subdev_set_fmt()  local
1270 		mbus = &q->subdev_fmt;  in cio2_subdev_set_fmt()
1285 	mutex_lock(&q->subdev_lock);  in cio2_subdev_set_fmt()
1287 	mutex_unlock(&q->subdev_lock);  in cio2_subdev_set_fmt()
1323 	struct cio2_queue *q = container_of(vd, struct cio2_queue, vdev);  in cio2_video_link_validate()  local
1338 	if (source_fmt.format.width != q->format.width ||  in cio2_video_link_validate()
1339 	    source_fmt.format.height != q->format.height) {  in cio2_video_link_validate()
1341 			q->format.width, q->format.height,  in cio2_video_link_validate()
1346 	if (!cio2_find_format(&q->format.pixelformat, &source_fmt.format.code))  in cio2_video_link_validate()
1390 	struct cio2_queue *q;  in cio2_notifier_bound()  local
1400 	q = &cio2->queue[s_asd->csi2.port];  in cio2_notifier_bound()
1402 	q->csi2 = s_asd->csi2;  in cio2_notifier_bound()
1403 	q->sensor = sd;  in cio2_notifier_bound()
1404 	q->csi_rx_base = cio2->base + CIO2_REG_PIPE_BASE(q->csi2.port);  in cio2_notifier_bound()
1427 	struct cio2_queue *q;  in cio2_notifier_complete()  local
1432 		q = &cio2->queue[s_asd->csi2.port];  in cio2_notifier_complete()
1434 		ret = media_entity_get_fwnode_pad(&q->sensor->entity,  in cio2_notifier_complete()
1443 		ret = media_create_pad_link(&q->sensor->entity, ret,  in cio2_notifier_complete()
1444 					    &q->subdev.entity, CIO2_PAD_SINK,  in cio2_notifier_complete()
1448 				q->sensor->name, s_asd->asd.match.fwnode, ret);  in cio2_notifier_complete()
1525 static int cio2_queue_init(struct cio2_device *cio2, struct cio2_queue *q)  in cio2_queue_init()  argument
1531 	struct video_device *vdev = &q->vdev;  in cio2_queue_init()
1532 	struct vb2_queue *vbq = &q->vbq;  in cio2_queue_init()
1533 	struct v4l2_subdev *subdev = &q->subdev;  in cio2_queue_init()
1538 	mutex_init(&q->lock);  in cio2_queue_init()
1539 	mutex_init(&q->subdev_lock);  in cio2_queue_init()
1542 	fmt = &q->subdev_fmt;  in cio2_queue_init()
1548 	q->format.width = default_width;  in cio2_queue_init()
1549 	q->format.height = default_height;  in cio2_queue_init()
1550 	q->format.pixelformat = dflt_fmt.fourcc;  in cio2_queue_init()
1551 	q->format.colorspace = V4L2_COLORSPACE_RAW;  in cio2_queue_init()
1552 	q->format.field = V4L2_FIELD_NONE;  in cio2_queue_init()
1553 	q->format.num_planes = 1;  in cio2_queue_init()
1554 	q->format.plane_fmt[0].bytesperline =  in cio2_queue_init()
1555 				cio2_bytesperline(q->format.width);  in cio2_queue_init()
1556 	q->format.plane_fmt[0].sizeimage = q->format.plane_fmt[0].bytesperline *  in cio2_queue_init()
1557 						q->format.height;  in cio2_queue_init()
1560 	r = cio2_fbpt_init(cio2, q);  in cio2_queue_init()
1565 	q->subdev_pads[CIO2_PAD_SINK].flags = MEDIA_PAD_FL_SINK |  in cio2_queue_init()
1567 	q->subdev_pads[CIO2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;  in cio2_queue_init()
1570 	r = media_entity_pads_init(&subdev->entity, CIO2_PADS, q->subdev_pads);  in cio2_queue_init()
1576 	q->vdev_pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;  in cio2_queue_init()
1578 	r = media_entity_pads_init(&vdev->entity, 1, &q->vdev_pad);  in cio2_queue_init()
1590 		 CIO2_ENTITY_NAME " %td", q - cio2->queue);  in cio2_queue_init()
1608 	vbq->lock = &q->lock;  in cio2_queue_init()
1617 		 "%s %td", CIO2_NAME, q - cio2->queue);  in cio2_queue_init()
1623 	vdev->queue = &q->vbq;  in cio2_queue_init()
1642 	vb2_video_unregister_device(&q->vdev);  in cio2_queue_init()
1650 	cio2_fbpt_exit(q, dev);  in cio2_queue_init()
1652 	mutex_destroy(&q->subdev_lock);  in cio2_queue_init()
1653 	mutex_destroy(&q->lock);  in cio2_queue_init()
1658 static void cio2_queue_exit(struct cio2_device *cio2, struct cio2_queue *q)  in cio2_queue_exit()  argument
1660 	vb2_video_unregister_device(&q->vdev);  in cio2_queue_exit()
1661 	media_entity_cleanup(&q->vdev.entity);  in cio2_queue_exit()
1662 	v4l2_device_unregister_subdev(&q->subdev);  in cio2_queue_exit()
1663 	media_entity_cleanup(&q->subdev.entity);  in cio2_queue_exit()
1664 	cio2_fbpt_exit(q, &cio2->pci_dev->dev);  in cio2_queue_exit()
1665 	mutex_destroy(&q->subdev_lock);  in cio2_queue_exit()
1666 	mutex_destroy(&q->lock);  in cio2_queue_exit()
1941 static void cio2_fbpt_rearrange(struct cio2_device *cio2, struct cio2_queue *q)  in cio2_fbpt_rearrange()  argument
1945 	for (i = 0, j = q->bufs_first; i < CIO2_MAX_BUFFERS;  in cio2_fbpt_rearrange()
1947 		if (q->bufs[j])  in cio2_fbpt_rearrange()
1954 		arrange(q->fbpt, sizeof(struct cio2_fbpt_entry) * CIO2_MAX_LOPS,  in cio2_fbpt_rearrange()
1956 		arrange(q->bufs, sizeof(struct cio2_buffer *),  in cio2_fbpt_rearrange()
1968 		cio2_fbpt_entry_enable(cio2, q->fbpt + i * CIO2_MAX_LOPS);  in cio2_fbpt_rearrange()
1975 	struct cio2_queue *q = cio2->cur_queue;  in cio2_suspend()  local
1983 	r = v4l2_subdev_call(q->sensor, video, s_stream, 0);  in cio2_suspend()
1989 	cio2_hw_exit(cio2, q);  in cio2_suspend()
1998 	cio2_fbpt_rearrange(cio2, q);  in cio2_suspend()
1999 	q->bufs_first = 0;  in cio2_suspend()
2000 	q->bufs_next = 0;  in cio2_suspend()
2008 	struct cio2_queue *q = cio2->cur_queue;  in cio2_resume()  local
2021 	r = cio2_hw_init(cio2, q);  in cio2_resume()
2027 	r = v4l2_subdev_call(q->sensor, video, s_stream, 1);  in cio2_resume()
2030 		cio2_hw_exit(cio2, q);  in cio2_resume()