1// SPDX-License-Identifier: GPL-2.0-only 2/* 3 * Copyright (C) 2013--2024 Intel Corporation 4 */ 5#include <linux/atomic.h> 6#include <linux/bug.h> 7#include <linux/device.h> 8#include <linux/list.h> 9#include <linux/lockdep.h> 10#include <linux/mutex.h> 11#include <linux/spinlock.h> 12#include <linux/types.h> 13 14#include <media/media-entity.h> 15#include <media/v4l2-subdev.h> 16#include <media/videobuf2-dma-contig.h> 17#include <media/videobuf2-v4l2.h> 18 19#include "ipu6-bus.h" 20#include "ipu6-fw-isys.h" 21#include "ipu6-isys.h" 22#include "ipu6-isys-video.h" 23 24static int queue_setup(struct vb2_queue *q, unsigned int *num_buffers, 25 unsigned int *num_planes, unsigned int sizes[], 26 struct device *alloc_devs[]) 27{ 28 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q); 29 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 30 struct device *dev = &av->isys->adev->auxdev.dev; 31 u32 size = ipu6_isys_get_data_size(av); 32 33 /* num_planes == 0: we're being called through VIDIOC_REQBUFS */ 34 if (!*num_planes) { 35 sizes[0] = size; 36 } else if (sizes[0] < size) { 37 dev_dbg(dev, "%s: queue setup: size %u < %u\n", 38 av->vdev.name, sizes[0], size); 39 return -EINVAL; 40 } 41 42 *num_planes = 1; 43 44 return 0; 45} 46 47static int ipu6_isys_buf_prepare(struct vb2_buffer *vb) 48{ 49 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue); 50 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 51 struct device *dev = &av->isys->adev->auxdev.dev; 52 u32 bytesperline = ipu6_isys_get_bytes_per_line(av); 53 u32 height = ipu6_isys_get_frame_height(av); 54 u32 size = ipu6_isys_get_data_size(av); 55 56 dev_dbg(dev, "buffer: %s: configured size %u, buffer size %lu\n", 57 av->vdev.name, size, vb2_plane_size(vb, 0)); 58 59 if (size > vb2_plane_size(vb, 0)) 60 return -EINVAL; 61 62 vb2_set_plane_payload(vb, 0, bytesperline * height); 63 64 return 0; 65} 66 67/* 68 * Queue a buffer list back to incoming or active queues. The buffers 69 * are removed from the buffer list. 70 */ 71void ipu6_isys_buffer_list_queue(struct ipu6_isys_buffer_list *bl, 72 unsigned long op_flags, 73 enum vb2_buffer_state state) 74{ 75 struct ipu6_isys_buffer *ib, *ib_safe; 76 unsigned long flags; 77 bool first = true; 78 79 if (!bl) 80 return; 81 82 WARN_ON_ONCE(!bl->nbufs); 83 WARN_ON_ONCE(op_flags & IPU6_ISYS_BUFFER_LIST_FL_ACTIVE && 84 op_flags & IPU6_ISYS_BUFFER_LIST_FL_INCOMING); 85 86 list_for_each_entry_safe(ib, ib_safe, &bl->head, head) { 87 struct ipu6_isys_video *av; 88 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib); 89 struct ipu6_isys_queue *aq = 90 vb2_queue_to_isys_queue(vb->vb2_queue); 91 struct device *dev; 92 93 av = ipu6_isys_queue_to_video(aq); 94 dev = &av->isys->adev->auxdev.dev; 95 spin_lock_irqsave(&aq->lock, flags); 96 list_del(&ib->head); 97 if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_ACTIVE) 98 list_add(&ib->head, &aq->active); 99 else if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_INCOMING) 100 list_add_tail(&ib->head, &aq->incoming); 101 spin_unlock_irqrestore(&aq->lock, flags); 102 103 if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_SET_STATE) 104 vb2_buffer_done(vb, state); 105 106 if (first) { 107 dev_dbg(dev, 108 "queue buf list %p flags %lx, s %d, %d bufs\n", 109 bl, op_flags, state, bl->nbufs); 110 first = false; 111 } 112 113 bl->nbufs--; 114 } 115 116 WARN_ON(bl->nbufs); 117} 118 119/* 120 * flush_firmware_streamon_fail() - Flush in cases where requests may 121 * have been queued to firmware and the *firmware streamon fails for a 122 * reason or another. 123 */ 124static void flush_firmware_streamon_fail(struct ipu6_isys_stream *stream) 125{ 126 struct device *dev = &stream->isys->adev->auxdev.dev; 127 struct ipu6_isys_queue *aq; 128 unsigned long flags; 129 130 lockdep_assert_held(&stream->mutex); 131 132 list_for_each_entry(aq, &stream->queues, node) { 133 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 134 struct ipu6_isys_buffer *ib, *ib_safe; 135 136 spin_lock_irqsave(&aq->lock, flags); 137 list_for_each_entry_safe(ib, ib_safe, &aq->active, head) { 138 struct vb2_buffer *vb = 139 ipu6_isys_buffer_to_vb2_buffer(ib); 140 141 list_del(&ib->head); 142 if (av->streaming) { 143 dev_dbg(dev, 144 "%s: queue buffer %u back to incoming\n", 145 av->vdev.name, vb->index); 146 /* Queue already streaming, return to driver. */ 147 list_add(&ib->head, &aq->incoming); 148 continue; 149 } 150 /* Queue not yet streaming, return to user. */ 151 dev_dbg(dev, "%s: return %u back to videobuf2\n", 152 av->vdev.name, vb->index); 153 vb2_buffer_done(ipu6_isys_buffer_to_vb2_buffer(ib), 154 VB2_BUF_STATE_QUEUED); 155 } 156 spin_unlock_irqrestore(&aq->lock, flags); 157 } 158} 159 160/* 161 * Attempt obtaining a buffer list from the incoming queues, a list of buffers 162 * that contains one entry from each video buffer queue. If a buffer can't be 163 * obtained from every queue, the buffers are returned back to the queue. 164 */ 165static int buffer_list_get(struct ipu6_isys_stream *stream, 166 struct ipu6_isys_buffer_list *bl) 167{ 168 struct device *dev = &stream->isys->adev->auxdev.dev; 169 struct ipu6_isys_queue *aq; 170 unsigned long flags; 171 unsigned long buf_flag = IPU6_ISYS_BUFFER_LIST_FL_INCOMING; 172 173 bl->nbufs = 0; 174 INIT_LIST_HEAD(&bl->head); 175 176 list_for_each_entry(aq, &stream->queues, node) { 177 struct ipu6_isys_buffer *ib; 178 179 spin_lock_irqsave(&aq->lock, flags); 180 if (list_empty(&aq->incoming)) { 181 spin_unlock_irqrestore(&aq->lock, flags); 182 if (!list_empty(&bl->head)) 183 ipu6_isys_buffer_list_queue(bl, buf_flag, 0); 184 return -ENODATA; 185 } 186 187 ib = list_last_entry(&aq->incoming, 188 struct ipu6_isys_buffer, head); 189 190 dev_dbg(dev, "buffer: %s: buffer %u\n", 191 ipu6_isys_queue_to_video(aq)->vdev.name, 192 ipu6_isys_buffer_to_vb2_buffer(ib)->index); 193 list_del(&ib->head); 194 list_add(&ib->head, &bl->head); 195 spin_unlock_irqrestore(&aq->lock, flags); 196 197 bl->nbufs++; 198 } 199 200 dev_dbg(dev, "get buffer list %p, %u buffers\n", bl, bl->nbufs); 201 202 return 0; 203} 204 205static void 206ipu6_isys_buf_to_fw_frame_buf_pin(struct vb2_buffer *vb, 207 struct ipu6_fw_isys_frame_buff_set_abi *set) 208{ 209 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue); 210 211 set->output_pins[aq->fw_output].addr = 212 vb2_dma_contig_plane_dma_addr(vb, 0); 213 set->output_pins[aq->fw_output].out_buf_id = vb->index + 1; 214} 215 216/* 217 * Convert a buffer list to a isys fw ABI framebuffer set. The 218 * buffer list is not modified. 219 */ 220#define IPU6_ISYS_FRAME_NUM_THRESHOLD (30) 221void 222ipu6_isys_buf_to_fw_frame_buf(struct ipu6_fw_isys_frame_buff_set_abi *set, 223 struct ipu6_isys_stream *stream, 224 struct ipu6_isys_buffer_list *bl) 225{ 226 struct ipu6_isys_buffer *ib; 227 228 WARN_ON(!bl->nbufs); 229 230 set->send_irq_sof = 1; 231 set->send_resp_sof = 1; 232 set->send_irq_eof = 0; 233 set->send_resp_eof = 0; 234 235 if (stream->streaming) 236 set->send_irq_capture_ack = 0; 237 else 238 set->send_irq_capture_ack = 1; 239 set->send_irq_capture_done = 0; 240 241 set->send_resp_capture_ack = 1; 242 set->send_resp_capture_done = 1; 243 if (atomic_read(&stream->sequence) >= IPU6_ISYS_FRAME_NUM_THRESHOLD) { 244 set->send_resp_capture_ack = 0; 245 set->send_resp_capture_done = 0; 246 } 247 248 list_for_each_entry(ib, &bl->head, head) { 249 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib); 250 251 ipu6_isys_buf_to_fw_frame_buf_pin(vb, set); 252 } 253} 254 255/* Start streaming for real. The buffer list must be available. */ 256static int ipu6_isys_stream_start(struct ipu6_isys_video *av, 257 struct ipu6_isys_buffer_list *bl, bool error) 258{ 259 struct ipu6_isys_stream *stream = av->stream; 260 struct device *dev = &stream->isys->adev->auxdev.dev; 261 struct ipu6_isys_buffer_list __bl; 262 int ret; 263 264 mutex_lock(&stream->isys->stream_mutex); 265 ret = ipu6_isys_video_set_streaming(av, 1, bl); 266 mutex_unlock(&stream->isys->stream_mutex); 267 if (ret) 268 goto out_requeue; 269 270 stream->streaming = 1; 271 272 bl = &__bl; 273 274 do { 275 struct ipu6_fw_isys_frame_buff_set_abi *buf = NULL; 276 struct isys_fw_msgs *msg; 277 u16 send_type = IPU6_FW_ISYS_SEND_TYPE_STREAM_CAPTURE; 278 279 ret = buffer_list_get(stream, bl); 280 if (ret < 0) 281 break; 282 283 msg = ipu6_get_fw_msg_buf(stream); 284 if (!msg) 285 return -ENOMEM; 286 287 buf = &msg->fw_msg.frame; 288 ipu6_isys_buf_to_fw_frame_buf(buf, stream, bl); 289 ipu6_fw_isys_dump_frame_buff_set(dev, buf, 290 stream->nr_output_pins); 291 ipu6_isys_buffer_list_queue(bl, IPU6_ISYS_BUFFER_LIST_FL_ACTIVE, 292 0); 293 ret = ipu6_fw_isys_complex_cmd(stream->isys, 294 stream->stream_handle, buf, 295 msg->dma_addr, sizeof(*buf), 296 send_type); 297 } while (!WARN_ON(ret)); 298 299 return 0; 300 301out_requeue: 302 if (bl && bl->nbufs) 303 ipu6_isys_buffer_list_queue(bl, 304 (IPU6_ISYS_BUFFER_LIST_FL_INCOMING | 305 error) ? 306 IPU6_ISYS_BUFFER_LIST_FL_SET_STATE : 307 0, error ? VB2_BUF_STATE_ERROR : 308 VB2_BUF_STATE_QUEUED); 309 flush_firmware_streamon_fail(stream); 310 311 return ret; 312} 313 314static void buf_queue(struct vb2_buffer *vb) 315{ 316 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue); 317 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 318 struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb); 319 struct ipu6_isys_video_buffer *ivb = 320 vb2_buffer_to_ipu6_isys_video_buffer(vvb); 321 struct ipu6_isys_buffer *ib = &ivb->ib; 322 struct device *dev = &av->isys->adev->auxdev.dev; 323 struct media_pipeline *media_pipe = 324 media_entity_pipeline(&av->vdev.entity); 325 struct ipu6_fw_isys_frame_buff_set_abi *buf = NULL; 326 struct ipu6_isys_stream *stream = av->stream; 327 struct ipu6_isys_buffer_list bl; 328 struct isys_fw_msgs *msg; 329 unsigned long flags; 330 dma_addr_t dma; 331 int ret; 332 333 dev_dbg(dev, "queue buffer %u for %s\n", vb->index, av->vdev.name); 334 335 dma = vb2_dma_contig_plane_dma_addr(vb, 0); 336 dev_dbg(dev, "iova: iova %pad\n", &dma); 337 338 spin_lock_irqsave(&aq->lock, flags); 339 list_add(&ib->head, &aq->incoming); 340 spin_unlock_irqrestore(&aq->lock, flags); 341 342 if (!media_pipe || !vb->vb2_queue->start_streaming_called) { 343 dev_dbg(dev, "media pipeline is not ready for %s\n", 344 av->vdev.name); 345 return; 346 } 347 348 mutex_lock(&stream->mutex); 349 350 if (stream->nr_streaming != stream->nr_queues) { 351 dev_dbg(dev, "not streaming yet, adding to incoming\n"); 352 goto out; 353 } 354 355 /* 356 * We just put one buffer to the incoming list of this queue 357 * (above). Let's see whether all queues in the pipeline would 358 * have a buffer. 359 */ 360 ret = buffer_list_get(stream, &bl); 361 if (ret < 0) { 362 dev_dbg(dev, "No buffers available\n"); 363 goto out; 364 } 365 366 msg = ipu6_get_fw_msg_buf(stream); 367 if (!msg) { 368 ret = -ENOMEM; 369 goto out; 370 } 371 372 buf = &msg->fw_msg.frame; 373 ipu6_isys_buf_to_fw_frame_buf(buf, stream, &bl); 374 ipu6_fw_isys_dump_frame_buff_set(dev, buf, stream->nr_output_pins); 375 376 if (!stream->streaming) { 377 ret = ipu6_isys_stream_start(av, &bl, true); 378 if (ret) 379 dev_err(dev, "stream start failed.\n"); 380 goto out; 381 } 382 383 /* 384 * We must queue the buffers in the buffer list to the 385 * appropriate video buffer queues BEFORE passing them to the 386 * firmware since we could get a buffer event back before we 387 * have queued them ourselves to the active queue. 388 */ 389 ipu6_isys_buffer_list_queue(&bl, IPU6_ISYS_BUFFER_LIST_FL_ACTIVE, 0); 390 391 ret = ipu6_fw_isys_complex_cmd(stream->isys, stream->stream_handle, 392 buf, msg->dma_addr, sizeof(*buf), 393 IPU6_FW_ISYS_SEND_TYPE_STREAM_CAPTURE); 394 if (ret < 0) 395 dev_err(dev, "send stream capture failed\n"); 396 397out: 398 mutex_unlock(&stream->mutex); 399} 400 401static int ipu6_isys_link_fmt_validate(struct ipu6_isys_queue *aq) 402{ 403 struct v4l2_mbus_framefmt format; 404 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 405 struct device *dev = &av->isys->adev->auxdev.dev; 406 struct media_pad *remote_pad = 407 media_pad_remote_pad_first(av->vdev.entity.pads); 408 struct v4l2_subdev *sd; 409 u32 r_stream, code; 410 int ret; 411 412 if (!remote_pad) 413 return -ENOTCONN; 414 415 sd = media_entity_to_v4l2_subdev(remote_pad->entity); 416 r_stream = ipu6_isys_get_src_stream_by_src_pad(sd, remote_pad->index); 417 418 ret = ipu6_isys_get_stream_pad_fmt(sd, remote_pad->index, r_stream, 419 &format); 420 421 if (ret) { 422 dev_dbg(dev, "failed to get %s: pad %d, stream:%d format\n", 423 sd->entity.name, remote_pad->index, r_stream); 424 return ret; 425 } 426 427 if (format.width != ipu6_isys_get_frame_width(av) || 428 format.height != ipu6_isys_get_frame_height(av)) { 429 dev_dbg(dev, "wrong width or height %ux%u (%ux%u expected)\n", 430 ipu6_isys_get_frame_width(av), 431 ipu6_isys_get_frame_height(av), format.width, 432 format.height); 433 return -EINVAL; 434 } 435 436 code = ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0)->code; 437 if (format.code != code) { 438 dev_dbg(dev, "wrong mbus code 0x%8.8x (0x%8.8x expected)\n", 439 code, format.code); 440 return -EINVAL; 441 } 442 443 return 0; 444} 445 446static void return_buffers(struct ipu6_isys_queue *aq, 447 enum vb2_buffer_state state) 448{ 449 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 450 struct ipu6_isys_buffer *ib; 451 bool need_reset = false; 452 unsigned long flags; 453 454 spin_lock_irqsave(&aq->lock, flags); 455 while (!list_empty(&aq->incoming)) { 456 struct vb2_buffer *vb; 457 458 ib = list_first_entry(&aq->incoming, struct ipu6_isys_buffer, 459 head); 460 vb = ipu6_isys_buffer_to_vb2_buffer(ib); 461 list_del(&ib->head); 462 spin_unlock_irqrestore(&aq->lock, flags); 463 464 vb2_buffer_done(vb, state); 465 466 spin_lock_irqsave(&aq->lock, flags); 467 } 468 469 /* 470 * Something went wrong (FW crash / HW hang / not all buffers 471 * returned from isys) if there are still buffers queued in active 472 * queue. We have to clean up places a bit. 473 */ 474 while (!list_empty(&aq->active)) { 475 struct vb2_buffer *vb; 476 477 ib = list_first_entry(&aq->active, struct ipu6_isys_buffer, 478 head); 479 vb = ipu6_isys_buffer_to_vb2_buffer(ib); 480 481 list_del(&ib->head); 482 spin_unlock_irqrestore(&aq->lock, flags); 483 484 vb2_buffer_done(vb, state); 485 486 spin_lock_irqsave(&aq->lock, flags); 487 need_reset = true; 488 } 489 490 spin_unlock_irqrestore(&aq->lock, flags); 491 492 if (need_reset) { 493 mutex_lock(&av->isys->mutex); 494 av->isys->need_reset = true; 495 mutex_unlock(&av->isys->mutex); 496 } 497} 498 499static void ipu6_isys_stream_cleanup(struct ipu6_isys_video *av) 500{ 501 video_device_pipeline_stop(&av->vdev); 502 ipu6_isys_put_stream(av->stream); 503 av->stream = NULL; 504} 505 506static int start_streaming(struct vb2_queue *q, unsigned int count) 507{ 508 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q); 509 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 510 struct device *dev = &av->isys->adev->auxdev.dev; 511 const struct ipu6_isys_pixelformat *pfmt = 512 ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0); 513 struct ipu6_isys_buffer_list __bl, *bl = NULL; 514 struct ipu6_isys_stream *stream; 515 struct media_entity *source_entity = NULL; 516 int nr_queues, ret; 517 518 dev_dbg(dev, "stream: %s: width %u, height %u, css pixelformat %u\n", 519 av->vdev.name, ipu6_isys_get_frame_width(av), 520 ipu6_isys_get_frame_height(av), pfmt->css_pixelformat); 521 522 ret = ipu6_isys_setup_video(av, &source_entity, &nr_queues); 523 if (ret < 0) { 524 dev_dbg(dev, "failed to setup video\n"); 525 goto out_return_buffers; 526 } 527 528 ret = ipu6_isys_link_fmt_validate(aq); 529 if (ret) { 530 dev_dbg(dev, 531 "%s: link format validation failed (%d)\n", 532 av->vdev.name, ret); 533 goto out_pipeline_stop; 534 } 535 536 ret = ipu6_isys_fw_open(av->isys); 537 if (ret) 538 goto out_pipeline_stop; 539 540 stream = av->stream; 541 mutex_lock(&stream->mutex); 542 if (!stream->nr_streaming) { 543 ret = ipu6_isys_video_prepare_stream(av, source_entity, 544 nr_queues); 545 if (ret) 546 goto out_fw_close; 547 } 548 549 stream->nr_streaming++; 550 dev_dbg(dev, "queue %u of %u\n", stream->nr_streaming, 551 stream->nr_queues); 552 553 list_add(&aq->node, &stream->queues); 554 ipu6_isys_set_csi2_streams_status(av, true); 555 ipu6_isys_configure_stream_watermark(av, true); 556 ipu6_isys_update_stream_watermark(av, true); 557 558 if (stream->nr_streaming != stream->nr_queues) 559 goto out; 560 561 bl = &__bl; 562 ret = buffer_list_get(stream, bl); 563 if (ret < 0) { 564 dev_warn(dev, "no buffer available, DRIVER BUG?\n"); 565 goto out; 566 } 567 568 ret = ipu6_isys_stream_start(av, bl, false); 569 if (ret) 570 goto out_stream_start; 571 572out: 573 mutex_unlock(&stream->mutex); 574 575 return 0; 576 577out_stream_start: 578 ipu6_isys_update_stream_watermark(av, false); 579 list_del(&aq->node); 580 stream->nr_streaming--; 581 582out_fw_close: 583 mutex_unlock(&stream->mutex); 584 ipu6_isys_fw_close(av->isys); 585 586out_pipeline_stop: 587 ipu6_isys_stream_cleanup(av); 588 589out_return_buffers: 590 return_buffers(aq, VB2_BUF_STATE_QUEUED); 591 592 return ret; 593} 594 595static void stop_streaming(struct vb2_queue *q) 596{ 597 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q); 598 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 599 struct ipu6_isys_stream *stream = av->stream; 600 601 ipu6_isys_set_csi2_streams_status(av, false); 602 603 mutex_lock(&stream->mutex); 604 605 ipu6_isys_update_stream_watermark(av, false); 606 607 mutex_lock(&av->isys->stream_mutex); 608 if (stream->nr_streaming == stream->nr_queues && stream->streaming) 609 ipu6_isys_video_set_streaming(av, 0, NULL); 610 mutex_unlock(&av->isys->stream_mutex); 611 612 stream->nr_streaming--; 613 list_del(&aq->node); 614 stream->streaming = 0; 615 mutex_unlock(&stream->mutex); 616 617 ipu6_isys_stream_cleanup(av); 618 619 return_buffers(aq, VB2_BUF_STATE_ERROR); 620 621 ipu6_isys_fw_close(av->isys); 622} 623 624static unsigned int 625get_sof_sequence_by_timestamp(struct ipu6_isys_stream *stream, 626 struct ipu6_fw_isys_resp_info_abi *info) 627{ 628 u64 time = (u64)info->timestamp[1] << 32 | info->timestamp[0]; 629 struct ipu6_isys *isys = stream->isys; 630 struct device *dev = &isys->adev->auxdev.dev; 631 unsigned int i; 632 633 /* 634 * The timestamp is invalid as no TSC in some FPGA platform, 635 * so get the sequence from pipeline directly in this case. 636 */ 637 if (time == 0) 638 return atomic_read(&stream->sequence) - 1; 639 640 for (i = 0; i < IPU6_ISYS_MAX_PARALLEL_SOF; i++) 641 if (time == stream->seq[i].timestamp) { 642 dev_dbg(dev, "sof: using seq nr %u for ts %llu\n", 643 stream->seq[i].sequence, time); 644 return stream->seq[i].sequence; 645 } 646 647 for (i = 0; i < IPU6_ISYS_MAX_PARALLEL_SOF; i++) 648 dev_dbg(dev, "sof: sequence %u, timestamp value %llu\n", 649 stream->seq[i].sequence, stream->seq[i].timestamp); 650 651 return 0; 652} 653 654static u64 get_sof_ns_delta(struct ipu6_isys_video *av, 655 struct ipu6_fw_isys_resp_info_abi *info) 656{ 657 struct ipu6_bus_device *adev = av->isys->adev; 658 struct ipu6_device *isp = adev->isp; 659 u64 delta, tsc_now; 660 661 ipu6_buttress_tsc_read(isp, &tsc_now); 662 if (!tsc_now) 663 return 0; 664 665 delta = tsc_now - ((u64)info->timestamp[1] << 32 | info->timestamp[0]); 666 667 return ipu6_buttress_tsc_ticks_to_ns(delta, isp); 668} 669 670void ipu6_isys_buf_calc_sequence_time(struct ipu6_isys_buffer *ib, 671 struct ipu6_fw_isys_resp_info_abi *info) 672{ 673 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib); 674 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 675 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue); 676 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 677 struct device *dev = &av->isys->adev->auxdev.dev; 678 struct ipu6_isys_stream *stream = av->stream; 679 u64 ns; 680 u32 sequence; 681 682 ns = ktime_get_ns() - get_sof_ns_delta(av, info); 683 sequence = get_sof_sequence_by_timestamp(stream, info); 684 685 vbuf->vb2_buf.timestamp = ns; 686 vbuf->sequence = sequence; 687 688 dev_dbg(dev, "buf: %s: buffer done, CPU-timestamp:%lld, sequence:%d\n", 689 av->vdev.name, ktime_get_ns(), sequence); 690 dev_dbg(dev, "index:%d, vbuf timestamp:%lld\n", vb->index, 691 vbuf->vb2_buf.timestamp); 692} 693 694void ipu6_isys_queue_buf_done(struct ipu6_isys_buffer *ib) 695{ 696 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib); 697 698 if (atomic_read(&ib->str2mmio_flag)) { 699 vb2_buffer_done(vb, VB2_BUF_STATE_ERROR); 700 /* 701 * Operation on buffer is ended with error and will be reported 702 * to the userspace when it is de-queued 703 */ 704 atomic_set(&ib->str2mmio_flag, 0); 705 } else { 706 vb2_buffer_done(vb, VB2_BUF_STATE_DONE); 707 } 708} 709 710void ipu6_isys_queue_buf_ready(struct ipu6_isys_stream *stream, 711 struct ipu6_fw_isys_resp_info_abi *info) 712{ 713 struct ipu6_isys_queue *aq = stream->output_pins[info->pin_id].aq; 714 struct ipu6_isys *isys = stream->isys; 715 struct device *dev = &isys->adev->auxdev.dev; 716 struct ipu6_isys_buffer *ib; 717 struct vb2_buffer *vb; 718 unsigned long flags; 719 bool first = true; 720 struct vb2_v4l2_buffer *buf; 721 722 spin_lock_irqsave(&aq->lock, flags); 723 if (list_empty(&aq->active)) { 724 spin_unlock_irqrestore(&aq->lock, flags); 725 dev_err(dev, "active queue empty\n"); 726 return; 727 } 728 729 list_for_each_entry_reverse(ib, &aq->active, head) { 730 dma_addr_t addr; 731 732 vb = ipu6_isys_buffer_to_vb2_buffer(ib); 733 addr = vb2_dma_contig_plane_dma_addr(vb, 0); 734 735 if (info->pin.addr != addr) { 736 if (first) 737 dev_err(dev, "Unexpected buffer address %pad\n", 738 &addr); 739 first = false; 740 continue; 741 } 742 743 if (info->error_info.error == 744 IPU6_FW_ISYS_ERROR_HW_REPORTED_STR2MMIO) { 745 /* 746 * Check for error message: 747 * 'IPU6_FW_ISYS_ERROR_HW_REPORTED_STR2MMIO' 748 */ 749 atomic_set(&ib->str2mmio_flag, 1); 750 } 751 dev_dbg(dev, "buffer: found buffer %pad\n", &addr); 752 753 buf = to_vb2_v4l2_buffer(vb); 754 buf->field = V4L2_FIELD_NONE; 755 756 list_del(&ib->head); 757 spin_unlock_irqrestore(&aq->lock, flags); 758 759 ipu6_isys_buf_calc_sequence_time(ib, info); 760 761 ipu6_isys_queue_buf_done(ib); 762 763 return; 764 } 765 766 dev_err(dev, "Failed to find a matching video buffer"); 767 768 spin_unlock_irqrestore(&aq->lock, flags); 769} 770 771static const struct vb2_ops ipu6_isys_queue_ops = { 772 .queue_setup = queue_setup, 773 .wait_prepare = vb2_ops_wait_prepare, 774 .wait_finish = vb2_ops_wait_finish, 775 .buf_prepare = ipu6_isys_buf_prepare, 776 .start_streaming = start_streaming, 777 .stop_streaming = stop_streaming, 778 .buf_queue = buf_queue, 779}; 780 781int ipu6_isys_queue_init(struct ipu6_isys_queue *aq) 782{ 783 struct ipu6_isys *isys = ipu6_isys_queue_to_video(aq)->isys; 784 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq); 785 int ret; 786 787 /* no support for userptr */ 788 if (!aq->vbq.io_modes) 789 aq->vbq.io_modes = VB2_MMAP | VB2_DMABUF; 790 791 aq->vbq.drv_priv = aq; 792 aq->vbq.ops = &ipu6_isys_queue_ops; 793 aq->vbq.lock = &av->mutex; 794 aq->vbq.mem_ops = &vb2_dma_contig_memops; 795 aq->vbq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 796 aq->vbq.min_queued_buffers = 1; 797 aq->vbq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 798 799 ret = vb2_queue_init(&aq->vbq); 800 if (ret) 801 return ret; 802 803 aq->dev = &isys->adev->auxdev.dev; 804 aq->vbq.dev = &isys->adev->auxdev.dev; 805 spin_lock_init(&aq->lock); 806 INIT_LIST_HEAD(&aq->active); 807 INIT_LIST_HEAD(&aq->incoming); 808 809 return 0; 810} 811