meta-egvirt/recipes-kernel/kernel-module-virtio-video/files/virtio_video_vq.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /* Driver for virtio video device.
   3  *
   4  * Copyright 2020 OpenSynergy GmbH.
   5  *
   6  * Based on drivers/gpu/drm/virtio/virtgpu_vq.c
   7  * Copyright (C) 2015 Red Hat, Inc.
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation; either version 2 of the License, or
  12  * (at your option) any later version.
  13  *
  14  * This program is distributed in the hope that it will be useful,
  15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  * GNU General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU General Public License
  20  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  21  */
  22
  23 #include "virtio_video.h"
  24
  25 #define MAX_INLINE_CMD_SIZE   298
  26 #define MAX_INLINE_RESP_SIZE  298
  27 #define VBUFFER_SIZE          (sizeof(struct virtio_video_vbuffer) \
  28                                + MAX_INLINE_CMD_SIZE               \
  29                                + MAX_INLINE_RESP_SIZE)
  30
  31 static int virtio_video_queue_event_buffer(struct virtio_video_device *vvd,
  32                                            struct virtio_video_event *evt);
  33 static void virtio_video_handle_event(struct virtio_video_device *vvd,
  34                                       struct virtio_video_event *evt);
  35
  36 void virtio_video_resource_id_get(struct virtio_video_device *vvd, uint32_t *id)
  37 {
  38         int handle;
  39
  40         idr_preload(GFP_KERNEL);
  41         spin_lock(&vvd->resource_idr_lock);
  42         handle = idr_alloc(&vvd->resource_idr, NULL, 1, 0, GFP_NOWAIT);
  43         spin_unlock(&vvd->resource_idr_lock);
  44         idr_preload_end();
  45         *id = handle;
  46 }
  47
  48 void virtio_video_resource_id_put(struct virtio_video_device *vvd, uint32_t id)
  49 {
  50         spin_lock(&vvd->resource_idr_lock);
  51         idr_remove(&vvd->resource_idr, id);
  52         spin_unlock(&vvd->resource_idr_lock);
  53 }
  54
  55 void virtio_video_stream_id_get(struct virtio_video_device *vvd,
  56                                 struct virtio_video_stream *stream,
  57                                 uint32_t *id)
  58 {
  59         int handle;
  60
  61         idr_preload(GFP_KERNEL);
  62         spin_lock(&vvd->stream_idr_lock);
  63         handle = idr_alloc(&vvd->stream_idr, stream, 1, 0, 0);
  64         spin_unlock(&vvd->stream_idr_lock);
  65         idr_preload_end();
  66         *id = handle;
  67 }
  68
  69 void virtio_video_stream_id_put(struct virtio_video_device *vvd, uint32_t id)
  70 {
  71         spin_lock(&vvd->stream_idr_lock);
  72         idr_remove(&vvd->stream_idr, id);
  73         spin_unlock(&vvd->stream_idr_lock);
  74 }
  75
  76 static bool vbuf_is_pending(struct virtio_video_device *vvd,
  77                             struct virtio_video_vbuffer *vbuf)
  78 {
  79         struct virtio_video_vbuffer *entry;
  80
  81         list_for_each_entry(entry, &vvd->pending_vbuf_list, pending_list_entry)
  82         {
  83                 if (entry == vbuf && entry->id == vbuf->id)
  84                         return true;
  85         }
  86
  87         return false;
  88 }
  89
  90 static void free_vbuf(struct virtio_video_device *vvd,
  91                       struct virtio_video_vbuffer *vbuf)
  92 {
  93         list_del(&vbuf->pending_list_entry);
  94         kfree(vbuf->data_buf);
  95         kmem_cache_free(vvd->vbufs, vbuf);
  96 }
  97
  98 void virtio_video_cmd_cb(struct virtqueue *vq)
  99 {
 100         struct virtio_video_device *vvd = vq->vdev->priv;
 101         struct virtio_video_vbuffer *vbuf;
 102         unsigned long flags;
 103         unsigned int len;
 104
 105         spin_lock_irqsave(&vvd->commandq.qlock, flags);
 106         while (vvd->commandq.ready) {
 107                 virtqueue_disable_cb(vq);
 108
 109                 while ((vbuf = virtqueue_get_buf(vq, &len))) {
 110                         if (!vbuf_is_pending(vvd, vbuf))
 111                                 continue;
 112
 113                         if (vbuf->resp_cb)
 114                                 vbuf->resp_cb(vvd, vbuf);
 115
 116                         if (vbuf->is_sync)
 117                                 complete(&vbuf->reclaimed);
 118                         else
 119                                 free_vbuf(vvd, vbuf);
 120                 }
 121
 122                 if (unlikely(virtqueue_is_broken(vq)))
 123                         break;
 124
 125                 if (virtqueue_enable_cb(vq))
 126                         break;
 127         }
 128         spin_unlock_irqrestore(&vvd->commandq.qlock, flags);
 129
 130         wake_up(&vvd->commandq.reclaim_queue);
 131 }
 132
 133 void virtio_video_process_events(struct work_struct *work)
 134 {
 135         struct virtio_video_device *vvd = container_of(work,
 136                         struct virtio_video_device, eventq.work);
 137         struct virtqueue *vq = vvd->eventq.vq;
 138         struct virtio_video_event *evt;
 139         unsigned int len;
 140
 141         while (vvd->eventq.ready) {
 142                 virtqueue_disable_cb(vq);
 143
 144                 while ((evt = virtqueue_get_buf(vq, &len))) {
 145                         virtio_video_handle_event(vvd, evt);
 146                         virtio_video_queue_event_buffer(vvd, evt);
 147                 }
 148
 149                 if (unlikely(virtqueue_is_broken(vq)))
 150                         break;
 151
 152                 if (virtqueue_enable_cb(vq))
 153                         break;
 154         }
 155 }
 156
 157 void virtio_video_event_cb(struct virtqueue *vq)
 158 {
 159         struct virtio_video_device *vvd = vq->vdev->priv;
 160
 161         schedule_work(&vvd->eventq.work);
 162 }
 163
 164 static struct virtio_video_vbuffer *
 165 virtio_video_get_vbuf(struct virtio_video_device *vvd, int size, int resp_size,
 166                       void *resp_buf, virtio_video_resp_cb resp_cb)
 167 {
 168         struct virtio_video_vbuffer *vbuf;
 169
 170         vbuf = kmem_cache_alloc(vvd->vbufs, GFP_KERNEL);
 171         if (!vbuf)
 172                 return ERR_PTR(-ENOMEM);
 173         memset(vbuf, 0, VBUFFER_SIZE);
 174
 175         BUG_ON(size > MAX_INLINE_CMD_SIZE);
 176         vbuf->buf = (void *)vbuf + sizeof(*vbuf);
 177         vbuf->size = size;
 178
 179         vbuf->resp_cb = resp_cb;
 180         vbuf->resp_size = resp_size;
 181         if (resp_size <= MAX_INLINE_RESP_SIZE && !resp_buf)
 182                 vbuf->resp_buf = (void *)vbuf->buf + size;
 183         else
 184                 vbuf->resp_buf = resp_buf;
 185         BUG_ON(!vbuf->resp_buf);
 186
 187         return vbuf;
 188 }
 189
 190 int virtio_video_alloc_vbufs(struct virtio_video_device *vvd)
 191 {
 192         vvd->vbufs =
 193                 kmem_cache_create("virtio-video-vbufs", VBUFFER_SIZE,
 194                                   __alignof__(struct virtio_video_vbuffer), 0,
 195                                   NULL);
 196         if (!vvd->vbufs)
 197                 return -ENOMEM;
 198
 199         return 0;
 200 }
 201
 202 void virtio_video_free_vbufs(struct virtio_video_device *vvd)
 203 {
 204         struct virtio_video_vbuffer *vbuf;
 205
 206         /* Release command buffers. Operation on vbufs here is lock safe,
 207            since before device was deinitialized and queues was stopped
 208            (in not ready state) */
 209         while ((vbuf = virtqueue_detach_unused_buf(vvd->commandq.vq))) {
 210                 if (vbuf_is_pending(vvd, vbuf))
 211                         free_vbuf(vvd, vbuf);
 212         }
 213
 214         kmem_cache_destroy(vvd->vbufs);
 215         vvd->vbufs = NULL;
 216
 217         /* Release event buffers */
 218         while (virtqueue_detach_unused_buf(vvd->eventq.vq));
 219
 220         kfree(vvd->evts);
 221         vvd->evts = NULL;
 222 }
 223
 224 static void *virtio_video_alloc_req(struct virtio_video_device *vvd,
 225                                     struct virtio_video_vbuffer **vbuffer_p,
 226                                     int size)
 227 {
 228         struct virtio_video_vbuffer *vbuf;
 229
 230         vbuf = virtio_video_get_vbuf(vvd, size,
 231                                      sizeof(struct virtio_video_cmd_hdr),
 232                                      NULL, NULL);
 233         if (IS_ERR(vbuf)) {
 234                 *vbuffer_p = NULL;
 235                 return ERR_CAST(vbuf);
 236         }
 237         *vbuffer_p = vbuf;
 238
 239         return vbuf->buf;
 240 }
 241
 242 static void *
 243 virtio_video_alloc_req_resp(struct virtio_video_device *vvd,
 244                             virtio_video_resp_cb cb,
 245                             struct virtio_video_vbuffer **vbuffer_p,
 246                             int req_size, int resp_size,
 247                             void *resp_buf)
 248 {
 249         struct virtio_video_vbuffer *vbuf;
 250
 251         vbuf = virtio_video_get_vbuf(vvd, req_size, resp_size, resp_buf, cb);
 252         if (IS_ERR(vbuf)) {
 253                 *vbuffer_p = NULL;
 254                 return ERR_CAST(vbuf);
 255         }
 256         *vbuffer_p = vbuf;
 257
 258         return vbuf->buf;
 259 }
 260
 261 static int
 262 virtio_video_queue_cmd_buffer(struct virtio_video_device *vvd,
 263                               struct virtio_video_vbuffer *vbuf)
 264 {
 265         unsigned long flags;
 266         struct virtqueue *vq = vvd->commandq.vq;
 267         struct scatterlist *sgs[3], vreq, vout, vresp;
 268         int outcnt = 0, incnt = 0;
 269         int ret;
 270
 271         if (!vvd->commandq.ready)
 272                 return -ENODEV;
 273
 274         spin_lock_irqsave(&vvd->commandq.qlock, flags);
 275
 276         vbuf->id = vvd->vbufs_sent++;
 277         list_add_tail(&vbuf->pending_list_entry, &vvd->pending_vbuf_list);
 278
 279         sg_init_one(&vreq, vbuf->buf, vbuf->size);
 280         sgs[outcnt + incnt] = &vreq;
 281         outcnt++;
 282
 283         if (vbuf->data_size) {
 284                 sg_init_one(&vout, vbuf->data_buf, vbuf->data_size);
 285                 sgs[outcnt + incnt] = &vout;
 286                 outcnt++;
 287         }
 288
 289         if (vbuf->resp_size) {
 290                 sg_init_one(&vresp, vbuf->resp_buf, vbuf->resp_size);
 291                 sgs[outcnt + incnt] = &vresp;
 292                 incnt++;
 293         }
 294
 295 retry:
 296         ret = virtqueue_add_sgs(vq, sgs, outcnt, incnt, vbuf, GFP_ATOMIC);
 297         if (ret == -ENOSPC) {
 298                 spin_unlock_irqrestore(&vvd->commandq.qlock, flags);
 299                 wait_event(vvd->commandq.reclaim_queue, vq->num_free);
 300                 spin_lock_irqsave(&vvd->commandq.qlock, flags);
 301                 goto retry;
 302         } else {
 303                 virtqueue_kick(vq);
 304         }
 305
 306         spin_unlock_irqrestore(&vvd->commandq.qlock, flags);
 307
 308         return ret;
 309 }
 310
 311 static int
 312 virtio_video_queue_cmd_buffer_sync(struct virtio_video_device *vvd,
 313                                    struct virtio_video_vbuffer *vbuf)
 314 {
 315         int ret;
 316         unsigned long rem;
 317         unsigned long flags;
 318
 319         vbuf->is_sync = true;
 320         init_completion(&vbuf->reclaimed);
 321
 322         ret = virtio_video_queue_cmd_buffer(vvd, vbuf);
 323         if (ret)
 324                 return ret;
 325
 326         rem = wait_for_completion_timeout(&vbuf->reclaimed, 5 * HZ);
 327         if (rem == 0)
 328                 ret = -ETIMEDOUT;
 329
 330         spin_lock_irqsave(&vvd->commandq.qlock, flags);
 331         if (vbuf_is_pending(vvd, vbuf))
 332                 free_vbuf(vvd, vbuf);
 333         spin_unlock_irqrestore(&vvd->commandq.qlock, flags);
 334
 335         return ret;
 336 }
 337
 338 static int virtio_video_queue_event_buffer(struct virtio_video_device *vvd,
 339                                            struct virtio_video_event *evt)
 340 {
 341         int ret;
 342         struct scatterlist sg;
 343         struct virtqueue *vq = vvd->eventq.vq;
 344
 345         memset(evt, 0, sizeof(struct virtio_video_event));
 346         sg_init_one(&sg, evt, sizeof(struct virtio_video_event));
 347
 348         ret = virtqueue_add_inbuf(vq, &sg, 1, evt, GFP_KERNEL);
 349         if (ret) {
 350                 v4l2_err(&vvd->v4l2_dev, "failed to queue event buffer\n");
 351                 return ret;
 352         }
 353
 354         virtqueue_kick(vq);
 355
 356         return 0;
 357 }
 358
 359 static void virtio_video_handle_event(struct virtio_video_device *vvd,
 360                                       struct virtio_video_event *evt)
 361 {
 362         int ret;
 363         struct virtio_video_stream *stream;
 364         uint32_t stream_id = evt->stream_id;
 365         struct video_device *vd = &vvd->video_dev;
 366
 367         mutex_lock(vd->lock);
 368
 369         stream = idr_find(&vvd->stream_idr, stream_id);
 370         if (!stream) {
 371                 v4l2_warn(&vvd->v4l2_dev, "stream_id=%u not found for event\n",
 372                           stream_id);
 373                 mutex_unlock(vd->lock);
 374                 return;
 375         }
 376
 377         switch (le32_to_cpu(evt->event_type)) {
 378         case VIRTIO_VIDEO_EVENT_DECODER_RESOLUTION_CHANGED:
 379                 v4l2_dbg(1, vvd->debug, &vvd->v4l2_dev,
 380                          "stream_id=%u: resolution change event\n", stream_id);
 381                 virtio_video_cmd_get_params(vvd, stream,
 382                                            VIRTIO_VIDEO_QUEUE_TYPE_OUTPUT);
 383                 virtio_video_queue_res_chg_event(stream);
 384                 if (virtio_video_state(stream) == STREAM_STATE_INIT) {
 385                         virtio_video_state_update(stream,
 386                                                   STREAM_STATE_DYNAMIC_RES_CHANGE);
 387                         wake_up(&vvd->wq);
 388                 }
 389                 break;
 390         case VIRTIO_VIDEO_EVENT_ERROR:
 391                 v4l2_err(&vvd->v4l2_dev, "stream_id=%i: error event\n",
 392                          stream_id);
 393                 virtio_video_state_update(stream, STREAM_STATE_ERROR);
 394                 virtio_video_handle_error(stream);
 395                 break;
 396         default:
 397                 v4l2_warn(&vvd->v4l2_dev, "stream_id=%i: unknown event\n",
 398                           stream_id);
 399                 break;
 400         }
 401
 402         mutex_unlock(vd->lock);
 403 }
 404
 405 int virtio_video_alloc_events(struct virtio_video_device *vvd)
 406 {
 407         int ret;
 408         size_t i;
 409         struct virtio_video_event *evts;
 410         size_t num =  vvd->eventq.vq->num_free;
 411
 412         evts = kzalloc(num * sizeof(struct virtio_video_event), GFP_KERNEL);
 413         if (!evts) {
 414                 v4l2_err(&vvd->v4l2_dev, "failed to alloc event buffers!!!\n");
 415                 return -ENOMEM;
 416         }
 417         vvd->evts = evts;
 418
 419         for (i = 0; i < num; i++) {
 420                 ret = virtio_video_queue_event_buffer(vvd, &evts[i]);
 421                 if (ret) {
 422                         v4l2_err(&vvd->v4l2_dev,
 423                                  "failed to queue event buffer\n");
 424                         return ret;
 425                 }
 426         }
 427
 428         return 0;
 429 }
 430
 431 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 432 int virtio_video_cmd_stream_create(struct virtio_video_device *vvd,
 433                                    uint32_t stream_id,
 434                                    enum virtio_video_format format,
 435                                    const char *tag)
 436 {
 437         struct virtio_video_stream_create *req_p;
 438         struct virtio_video_vbuffer *vbuf;
 439
 440         req_p = virtio_video_alloc_req(vvd, &vbuf, sizeof(*req_p));
 441         if (IS_ERR(req_p))
 442                 return PTR_ERR(req_p);
 443
 444         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_STREAM_CREATE);
 445         req_p->hdr.stream_id = cpu_to_le32(stream_id);
 446         req_p->in_mem_type = cpu_to_le32(VIRTIO_VIDEO_MEM_TYPE_GUEST_PAGES);
 447         req_p->out_mem_type = cpu_to_le32(VIRTIO_VIDEO_MEM_TYPE_GUEST_PAGES);
 448         req_p->coded_format = cpu_to_le32(format);
 449         if (strscpy(req_p->tag, tag, sizeof(req_p->tag) - 1) < 0)
 450                 v4l2_err(&vvd->v4l2_dev, "failed to copy stream tag\n");
 451         req_p->tag[sizeof(req_p->tag) - 1] = 0;
 452
 453         return virtio_video_queue_cmd_buffer(vvd, vbuf);
 454 }
 455
 456 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 457 int virtio_video_cmd_stream_destroy(struct virtio_video_device *vvd,
 458                                     uint32_t stream_id)
 459 {
 460         struct virtio_video_stream_destroy *req_p;
 461         struct virtio_video_vbuffer *vbuf;
 462
 463         req_p = virtio_video_alloc_req(vvd, &vbuf, sizeof(*req_p));
 464         if (IS_ERR(req_p))
 465                 return PTR_ERR(req_p);
 466
 467         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_STREAM_DESTROY);
 468         req_p->hdr.stream_id = cpu_to_le32(stream_id);
 469
 470         return virtio_video_queue_cmd_buffer(vvd, vbuf);
 471 }
 472
 473 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 474 int virtio_video_cmd_stream_drain(struct virtio_video_device *vvd,
 475                                   uint32_t stream_id)
 476 {
 477         struct virtio_video_stream_drain *req_p;
 478         struct virtio_video_vbuffer *vbuf;
 479
 480         req_p = virtio_video_alloc_req(vvd, &vbuf, sizeof(*req_p));
 481         if (IS_ERR(req_p))
 482                 return PTR_ERR(req_p);
 483
 484         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_STREAM_DRAIN);
 485         req_p->hdr.stream_id = cpu_to_le32(stream_id);
 486
 487         return virtio_video_queue_cmd_buffer(vvd, vbuf);
 488 }
 489
 490 int virtio_video_cmd_resource_attach(struct virtio_video_device *vvd,
 491                                      uint32_t stream_id, uint32_t resource_id,
 492                                      enum virtio_video_queue_type queue_type,
 493                                      void *buf, size_t buf_size)
 494 {
 495         struct virtio_video_resource_attach *req_p;
 496         struct virtio_video_vbuffer *vbuf;
 497
 498         req_p = virtio_video_alloc_req(vvd, &vbuf, sizeof(*req_p));
 499         if (IS_ERR(req_p))
 500                 return PTR_ERR(req_p);
 501
 502         req_p->cmd_type = cpu_to_le32(VIRTIO_VIDEO_CMD_RESOURCE_ATTACH);
 503         req_p->stream_id = cpu_to_le32(stream_id);
 504         req_p->queue_type = cpu_to_le32(queue_type);
 505         req_p->resource_id = cpu_to_le32(resource_id);
 506
 507         vbuf->data_buf = buf;
 508         vbuf->data_size = buf_size;
 509
 510         return virtio_video_queue_cmd_buffer(vvd, vbuf);
 511 }
 512
 513 int virtio_video_cmd_queue_detach_resources(struct virtio_video_device *vvd,
 514                                 struct virtio_video_stream *stream,
 515                                 enum virtio_video_queue_type queue_type)
 516 {
 517         int ret;
 518         struct virtio_video_queue_detach_resources *req_p;
 519         struct virtio_video_vbuffer *vbuf;
 520
 521         req_p = virtio_video_alloc_req(vvd, &vbuf, sizeof(*req_p));
 522         if (IS_ERR(req_p))
 523                 return PTR_ERR(req_p);
 524
 525         req_p->cmd_type = cpu_to_le32(VIRTIO_VIDEO_CMD_QUEUE_DETACH_RESOURCES);
 526         req_p->stream_id = cpu_to_le32(stream->stream_id);
 527         req_p->queue_type = cpu_to_le32(queue_type);
 528
 529         ret = virtio_video_queue_cmd_buffer_sync(vvd, vbuf);
 530         if (ret == -ETIMEDOUT)
 531                 v4l2_err(&vvd->v4l2_dev,
 532                          "timed out waiting for resource destruction for %s\n",
 533                          (queue_type == VIRTIO_VIDEO_QUEUE_TYPE_INPUT) ?
 534                          "OUTPUT" : "CAPTURE");
 535         return ret;
 536 }
 537
 538 static void
 539 virtio_video_cmd_resource_queue_cb(struct virtio_video_device *vvd,
 540                                    struct virtio_video_vbuffer *vbuf)
 541 {
 542         uint32_t flags;
 543         uint64_t timestamp;
 544         struct virtio_video_buffer *virtio_vb = vbuf->priv;
 545         struct virtio_video_resource_queue_resp *resp =
 546                 (struct virtio_video_resource_queue_resp *)vbuf->resp_buf;
 547
 548         flags = le32_to_cpu(resp->flags);
 549         timestamp = le64_to_cpu(resp->timestamp);
 550
 551         virtio_video_buf_done(virtio_vb, flags, timestamp, resp->data_sizes);
 552 }
 553
 554 int virtio_video_cmd_resource_queue(struct virtio_video_device *vvd,
 555                                     uint32_t stream_id,
 556                                     struct virtio_video_buffer *virtio_vb,
 557                                     uint32_t data_size[],
 558                                     uint8_t num_data_size,
 559                                     enum virtio_video_queue_type queue_type)
 560 {
 561         uint8_t i;
 562         struct virtio_video_resource_queue *req_p;
 563         struct virtio_video_resource_queue_resp *resp_p;
 564         struct virtio_video_vbuffer *vbuf;
 565         size_t resp_size = sizeof(struct virtio_video_resource_queue_resp);
 566
 567         req_p = virtio_video_alloc_req_resp(vvd,
 568                                             &virtio_video_cmd_resource_queue_cb,
 569                                             &vbuf, sizeof(*req_p), resp_size,
 570                                             NULL);
 571         if (IS_ERR(req_p))
 572                 return PTR_ERR(req_p);
 573
 574         req_p->cmd_type = cpu_to_le32(VIRTIO_VIDEO_CMD_RESOURCE_QUEUE);
 575         req_p->stream_id = cpu_to_le32(stream_id);
 576         req_p->queue_type = cpu_to_le32(queue_type);
 577         req_p->resource_id = cpu_to_le32(virtio_vb->resource_id);
 578         req_p->flags = 0;
 579         req_p->timestamp =
 580                 cpu_to_le64(virtio_vb->v4l2_m2m_vb.vb.vb2_buf.timestamp);
 581
 582         for (i = 0; i < num_data_size; ++i)
 583                 req_p->data_sizes[i] = cpu_to_le32(data_size[i]);
 584
 585         resp_p = (struct virtio_video_resource_queue_resp *)vbuf->resp_buf;
 586         memset(resp_p, 0, sizeof(*resp_p));
 587
 588         vbuf->priv = virtio_vb;
 589
 590         return virtio_video_queue_cmd_buffer(vvd, vbuf);
 591 }
 592
 593 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 594 int virtio_video_cmd_queue_clear(struct virtio_video_device *vvd,
 595                                  struct virtio_video_stream *stream,
 596                                  enum virtio_video_queue_type queue_type)
 597 {
 598         int ret;
 599         struct virtio_video_queue_clear *req_p;
 600         struct virtio_video_vbuffer *vbuf;
 601
 602         req_p = virtio_video_alloc_req(vvd, &vbuf, sizeof(*req_p));
 603         if (IS_ERR(req_p))
 604                 return PTR_ERR(req_p);
 605
 606         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_QUEUE_CLEAR);
 607         req_p->hdr.stream_id = cpu_to_le32(stream->stream_id);
 608         req_p->queue_type = cpu_to_le32(queue_type);
 609
 610         ret = virtio_video_queue_cmd_buffer_sync(vvd, vbuf);
 611         if (ret == -ETIMEDOUT)
 612                 v4l2_err(&vvd->v4l2_dev,
 613                          "timed out waiting for %s queue clear\n",
 614                          (queue_type == VIRTIO_VIDEO_QUEUE_TYPE_INPUT) ?
 615                          "OUTPUT" : "CAPTURE");
 616         return ret;
 617 }
 618
 619 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 620 int virtio_video_cmd_query_capability(struct virtio_video_device *vvd,
 621                                       void *resp_buf, size_t resp_size,
 622                                       enum virtio_video_queue_type queue_type)
 623 {
 624         int ret;
 625         struct virtio_video_query_capability *req_p;
 626         struct virtio_video_vbuffer *vbuf;
 627
 628         req_p = virtio_video_alloc_req_resp(vvd, NULL, &vbuf, sizeof(*req_p),
 629                                             resp_size, resp_buf);
 630         if (IS_ERR(req_p))
 631                 return PTR_ERR(req_p);
 632
 633         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_QUERY_CAPABILITY);
 634         req_p->queue_type = cpu_to_le32(queue_type);
 635
 636         ret = virtio_video_queue_cmd_buffer_sync(vvd, vbuf);
 637         if (ret == -ETIMEDOUT)
 638                 v4l2_err(&vvd->v4l2_dev,
 639                          "timed out waiting for capabilities for %s\n",
 640                          (queue_type == VIRTIO_VIDEO_QUEUE_TYPE_INPUT) ?
 641                          "OUTPUT" : "CAPTURE");
 642         return ret;
 643 }
 644
 645 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 646 int virtio_video_query_control_level(struct virtio_video_device *vvd,
 647                                      void *resp_buf, size_t resp_size,
 648                                      enum virtio_video_format format)
 649 {
 650         int ret;
 651         struct virtio_video_query_control *req_p;
 652         struct virtio_video_query_control_level *ctrl_l;
 653         struct virtio_video_vbuffer *vbuf;
 654         uint32_t req_size = 0;
 655
 656         req_size = sizeof(struct virtio_video_query_control) +
 657                 sizeof(struct virtio_video_query_control_level);
 658
 659         req_p = virtio_video_alloc_req_resp(vvd, NULL, &vbuf, req_size,
 660                                             resp_size, resp_buf);
 661         if (IS_ERR(req_p))
 662                 return PTR_ERR(req_p);
 663
 664         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_QUERY_CONTROL);
 665         req_p->control = cpu_to_le32(VIRTIO_VIDEO_CONTROL_LEVEL);
 666         ctrl_l = (void *)((char *)req_p +
 667                           sizeof(struct virtio_video_query_control));
 668         ctrl_l->format = cpu_to_le32(format);
 669
 670         ret = virtio_video_queue_cmd_buffer_sync(vvd, vbuf);
 671         if (ret == -ETIMEDOUT)
 672                 v4l2_err(&vvd->v4l2_dev,
 673                          "timed out waiting for level query\n");
 674         return ret;
 675 }
 676
 677 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 678 int virtio_video_query_control_profile(struct virtio_video_device *vvd,
 679                                        void *resp_buf, size_t resp_size,
 680                                        enum virtio_video_format format)
 681 {
 682         int ret;
 683         struct virtio_video_query_control *req_p;
 684         struct virtio_video_query_control_profile *ctrl_p;
 685         struct virtio_video_vbuffer *vbuf;
 686         uint32_t req_size = 0;
 687
 688         req_size = sizeof(struct virtio_video_query_control) +
 689                 sizeof(struct virtio_video_query_control_profile);
 690
 691         req_p = virtio_video_alloc_req_resp(vvd, NULL, &vbuf, req_size,
 692                                             resp_size, resp_buf);
 693         if (IS_ERR(req_p))
 694                 return PTR_ERR(req_p);
 695
 696         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_QUERY_CONTROL);
 697         req_p->control = cpu_to_le32(VIRTIO_VIDEO_CONTROL_PROFILE);
 698         ctrl_p = (void *)((char *)req_p +
 699                           sizeof(struct virtio_video_query_control));
 700         ctrl_p->format = cpu_to_le32(format);
 701
 702         ret = virtio_video_queue_cmd_buffer_sync(vvd, vbuf);
 703         if (ret == -ETIMEDOUT)
 704                 v4l2_err(&vvd->v4l2_dev,
 705                          "timed out waiting for profile query\n");
 706         return ret;
 707 }
 708
 709 static void
 710 virtio_video_cmd_get_params_cb(struct virtio_video_device *vvd,
 711                                struct virtio_video_vbuffer *vbuf)
 712 {
 713         int i;
 714         struct virtio_video_get_params_resp *resp =
 715                 (struct virtio_video_get_params_resp *)vbuf->resp_buf;
 716         struct virtio_video_params *params = &resp->params;
 717         struct virtio_video_stream *stream = vbuf->priv;
 718         enum virtio_video_queue_type queue_type;
 719         struct video_format_info *format_info;
 720
 721         queue_type = le32_to_cpu(params->queue_type);
 722         if (queue_type == VIRTIO_VIDEO_QUEUE_TYPE_INPUT)
 723                 format_info = &stream->in_info;
 724         else
 725                 format_info = &stream->out_info;
 726
 727         format_info->frame_rate = le32_to_cpu(params->frame_rate);
 728         format_info->frame_width = le32_to_cpu(params->frame_width);
 729         format_info->frame_height = le32_to_cpu(params->frame_height);
 730         format_info->min_buffers = le32_to_cpu(params->min_buffers);
 731         format_info->max_buffers = le32_to_cpu(params->max_buffers);
 732         format_info->fourcc_format =
 733                 virtio_video_format_to_v4l2(le32_to_cpu(params->format));
 734
 735         format_info->crop.top = le32_to_cpu(params->crop.top);
 736         format_info->crop.left = le32_to_cpu(params->crop.left);
 737         format_info->crop.width = le32_to_cpu(params->crop.width);
 738         format_info->crop.height = le32_to_cpu(params->crop.height);
 739
 740         format_info->num_planes = le32_to_cpu(params->num_planes);
 741         for (i = 0; i < le32_to_cpu(params->num_planes); i++) {
 742                 struct virtio_video_plane_format *plane_formats =
 743                                                  &params->plane_formats[i];
 744                 struct video_plane_format *plane_format =
 745                                                  &format_info->plane_format[i];
 746
 747                 plane_format->plane_size =
 748                                  le32_to_cpu(plane_formats->plane_size);
 749                 plane_format->stride = le32_to_cpu(plane_formats->stride);
 750         }
 751 }
 752
 753 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 754 int virtio_video_cmd_get_params(struct virtio_video_device *vvd,
 755                                 struct virtio_video_stream *stream,
 756                                 enum virtio_video_queue_type queue_type)
 757 {
 758         int ret;
 759         struct virtio_video_get_params *req_p;
 760         struct virtio_video_vbuffer *vbuf;
 761         struct virtio_video_get_params_resp *resp_p;
 762         size_t resp_size = sizeof(struct virtio_video_get_params_resp);
 763
 764         req_p = virtio_video_alloc_req_resp(vvd,
 765                                         &virtio_video_cmd_get_params_cb,
 766                                         &vbuf, sizeof(*req_p), resp_size,
 767                                         NULL);
 768         if (IS_ERR(req_p))
 769                 return PTR_ERR(req_p);
 770
 771         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_GET_PARAMS);
 772         req_p->hdr.stream_id = cpu_to_le32(stream->stream_id);
 773         req_p->queue_type = cpu_to_le32(queue_type);
 774
 775         resp_p = (struct virtio_video_get_params_resp *)vbuf->resp_buf;
 776
 777         vbuf->priv = stream;
 778
 779         ret = virtio_video_queue_cmd_buffer_sync(vvd, vbuf);
 780         if (ret == -ETIMEDOUT)
 781                 v4l2_err(&vvd->v4l2_dev,
 782                          "timed out waiting for get_params\n");
 783         return ret;
 784 }
 785
 786 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 787 int
 788 virtio_video_cmd_set_params(struct virtio_video_device *vvd,
 789                             struct virtio_video_stream *stream,
 790                             struct video_format_info *format_info,
 791                             enum virtio_video_queue_type queue_type)
 792 {
 793         int i;
 794         struct virtio_video_set_params *req_p;
 795         struct virtio_video_vbuffer *vbuf;
 796
 797         req_p = virtio_video_alloc_req(vvd, &vbuf, sizeof(*req_p));
 798         if (IS_ERR(req_p))
 799                 return PTR_ERR(req_p);
 800
 801         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_SET_PARAMS);
 802         req_p->hdr.stream_id = cpu_to_le32(stream->stream_id);
 803         req_p->params.queue_type = cpu_to_le32(queue_type);
 804         req_p->params.frame_rate = cpu_to_le32(format_info->frame_rate);
 805         req_p->params.frame_width = cpu_to_le32(format_info->frame_width);
 806         req_p->params.frame_height = cpu_to_le32(format_info->frame_height);
 807         req_p->params.format = virtio_video_v4l2_format_to_virtio(
 808                                  cpu_to_le32(format_info->fourcc_format));
 809         req_p->params.min_buffers = cpu_to_le32(format_info->min_buffers);
 810         req_p->params.max_buffers = cpu_to_le32(format_info->max_buffers);
 811         req_p->params.num_planes = cpu_to_le32(format_info->num_planes);
 812
 813         for (i = 0; i < format_info->num_planes; i++) {
 814                 struct virtio_video_plane_format *plane_formats =
 815                         &req_p->params.plane_formats[i];
 816                 struct video_plane_format *plane_format =
 817                         &format_info->plane_format[i];
 818                 plane_formats->plane_size =
 819                                  cpu_to_le32(plane_format->plane_size);
 820                 plane_formats->stride = cpu_to_le32(plane_format->stride);
 821         }
 822
 823         return virtio_video_queue_cmd_buffer(vvd, vbuf);
 824 }
 825
 826 static void
 827 virtio_video_cmd_get_ctrl_profile_cb(struct virtio_video_device *vvd,
 828                                      struct virtio_video_vbuffer *vbuf)
 829 {
 830         struct virtio_video_get_control_resp *resp =
 831                 (struct virtio_video_get_control_resp *)vbuf->resp_buf;
 832         struct virtio_video_control_val_profile *resp_p = NULL;
 833         struct virtio_video_stream *stream = vbuf->priv;
 834         struct video_control_info *control = &stream->control;
 835
 836         resp_p = (void *)((char *)resp +
 837                           sizeof(struct virtio_video_get_control_resp));
 838
 839         control->profile = le32_to_cpu(resp_p->profile);
 840 }
 841
 842 static void
 843 virtio_video_cmd_get_ctrl_level_cb(struct virtio_video_device *vvd,
 844                                    struct virtio_video_vbuffer *vbuf)
 845 {
 846         struct virtio_video_get_control_resp *resp =
 847                 (struct virtio_video_get_control_resp *)vbuf->resp_buf;
 848         struct virtio_video_control_val_level *resp_p;
 849         struct virtio_video_stream *stream = vbuf->priv;
 850         struct video_control_info *control = &stream->control;
 851
 852         resp_p = (void *)((char *)resp +
 853                           sizeof(struct virtio_video_get_control_resp));
 854
 855         control->level = le32_to_cpu(resp_p->level);
 856 }
 857
 858 static void
 859 virtio_video_cmd_get_ctrl_bitrate_cb(struct virtio_video_device *vvd,
 860                                      struct virtio_video_vbuffer *vbuf)
 861 {
 862         struct virtio_video_get_control_resp *resp =
 863                 (struct virtio_video_get_control_resp *)vbuf->resp_buf;
 864         struct virtio_video_control_val_bitrate *resp_p = NULL;
 865         struct virtio_video_stream *stream = vbuf->priv;
 866         struct video_control_info *control = &stream->control;
 867
 868         resp_p = (void *)((char *) resp +
 869                           sizeof(struct virtio_video_get_control_resp));
 870
 871         control->bitrate = le32_to_cpu(resp_p->bitrate);
 872 }
 873
 874 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 875 int virtio_video_cmd_get_control(struct virtio_video_device *vvd,
 876                                  struct virtio_video_stream *stream,
 877                                  enum virtio_video_control_type control)
 878 {
 879         int ret;
 880         struct virtio_video_get_control *req_p;
 881         struct virtio_video_get_control_resp *resp_p;
 882         struct virtio_video_vbuffer *vbuf;
 883         size_t resp_size = sizeof(struct virtio_video_get_control_resp);
 884         virtio_video_resp_cb cb;
 885
 886         switch (control) {
 887         case VIRTIO_VIDEO_CONTROL_PROFILE:
 888                 resp_size += sizeof(struct virtio_video_control_val_profile);
 889                 cb = &virtio_video_cmd_get_ctrl_profile_cb;
 890                 break;
 891         case VIRTIO_VIDEO_CONTROL_LEVEL:
 892                 resp_size += sizeof(struct virtio_video_control_val_level);
 893                 cb = &virtio_video_cmd_get_ctrl_level_cb;
 894                 break;
 895         case VIRTIO_VIDEO_CONTROL_BITRATE:
 896                 resp_size += sizeof(struct virtio_video_control_val_bitrate);
 897                 cb = &virtio_video_cmd_get_ctrl_bitrate_cb;
 898                 break;
 899         default:
 900                 return -EINVAL;
 901         }
 902
 903         req_p = virtio_video_alloc_req_resp(vvd, cb, &vbuf,
 904                                             sizeof(*req_p), resp_size, NULL);
 905         if (IS_ERR(req_p))
 906                 return PTR_ERR(req_p);
 907
 908         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_GET_CONTROL);
 909         req_p->hdr.stream_id = cpu_to_le32(stream->stream_id);
 910         req_p->control = cpu_to_le32(control);
 911
 912         resp_p = (struct virtio_video_get_control_resp *)vbuf->resp_buf;
 913
 914         vbuf->priv = stream;
 915
 916         ret = virtio_video_queue_cmd_buffer_sync(vvd, vbuf);
 917         if (ret == -ETIMEDOUT)
 918                 v4l2_err(&vvd->v4l2_dev,
 919                          "timed out waiting for get_control\n");
 920         return ret;
 921 }
 922
 923 // TODO: replace virtio_video_cmd_hdr accoring to specification v4
 924 int virtio_video_cmd_set_control(struct virtio_video_device *vvd,
 925                                  uint32_t stream_id,
 926                                  enum virtio_video_control_type control,
 927                                  uint32_t value)
 928 {
 929         struct virtio_video_set_control *req_p;
 930         struct virtio_video_vbuffer *vbuf;
 931         struct virtio_video_control_val_level *ctrl_l;
 932         struct virtio_video_control_val_profile *ctrl_p;
 933         struct virtio_video_control_val_bitrate *ctrl_b;
 934         size_t size;
 935
 936         if (value == 0)
 937                 return -EINVAL;
 938
 939         switch (control) {
 940         case VIRTIO_VIDEO_CONTROL_PROFILE:
 941                 size = sizeof(struct virtio_video_control_val_profile);
 942                 break;
 943         case VIRTIO_VIDEO_CONTROL_LEVEL:
 944                 size = sizeof(struct virtio_video_control_val_level);
 945                 break;
 946         case VIRTIO_VIDEO_CONTROL_BITRATE:
 947                 size = sizeof(struct virtio_video_control_val_bitrate);
 948                 break;
 949         default:
 950                 return -EINVAL;
 951         }
 952
 953         req_p = virtio_video_alloc_req(vvd, &vbuf, size + sizeof(*req_p));
 954         if (IS_ERR(req_p))
 955                 return PTR_ERR(req_p);
 956
 957         req_p->hdr.type = cpu_to_le32(VIRTIO_VIDEO_CMD_SET_CONTROL);
 958         req_p->hdr.stream_id = cpu_to_le32(stream_id);
 959         req_p->control = cpu_to_le32(control);
 960
 961         switch (control) {
 962         case VIRTIO_VIDEO_CONTROL_PROFILE:
 963                 ctrl_p = (void *)((char *)req_p +
 964                                   sizeof(struct virtio_video_set_control));
 965                 ctrl_p->profile = cpu_to_le32(value);
 966                 break;
 967         case VIRTIO_VIDEO_CONTROL_LEVEL:
 968                 ctrl_l = (void *)((char *)req_p +
 969                                  sizeof(struct virtio_video_set_control));
 970                 ctrl_l->level = cpu_to_le32(value);
 971                 break;
 972         case VIRTIO_VIDEO_CONTROL_BITRATE:
 973                 ctrl_b = (void *)((char *)req_p +
 974                                  sizeof(struct virtio_video_set_control));
 975                 ctrl_b->bitrate = cpu_to_le32(value);
 976                 break;
 977         }
 978
 979         return virtio_video_queue_cmd_buffer(vvd, vbuf);
 980 }
 981