drivers/media/pci/intel/ipu6/ipu6-isys-queue.c

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