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

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /* Driver for virtio video device.
   3  *
   4  * Copyright 2020 OpenSynergy GmbH.
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 #include <media/v4l2-ioctl.h>
  21 #include <media/videobuf2-dma-sg.h>
  22
  23 #include "virtio_video.h"
  24
  25 static void virtio_video_free_frame_rates(struct video_format_frame *frame)
  26 {
  27         kfree(frame->frame_rates);
  28 }
  29
  30 static void virtio_video_free_frames(struct video_format *fmt)
  31 {
  32         size_t idx = 0;
  33
  34         for (idx = 0; idx < fmt->desc.num_frames; idx++)
  35                 virtio_video_free_frame_rates(&fmt->frames[idx]);
  36         kfree(fmt->frames);
  37 }
  38
  39 static void virtio_video_free_fmt(struct list_head *fmts_list)
  40 {
  41         struct video_format *fmt, *tmp;
  42
  43         list_for_each_entry_safe(fmt, tmp, fmts_list, formats_list_entry) {
  44                 list_del(&fmt->formats_list_entry);
  45                 virtio_video_free_frames(fmt);
  46                 kfree(fmt);
  47         }
  48 }
  49
  50 static void virtio_video_free_fmts(struct virtio_video_device *vvd)
  51 {
  52         virtio_video_free_fmt(&vvd->input_fmt_list);
  53         virtio_video_free_fmt(&vvd->output_fmt_list);
  54 }
  55
  56 static void virtio_video_copy_fmt_range(struct virtio_video_format_range *d_rge,
  57                                         struct virtio_video_format_range *s_rge)
  58 {
  59         d_rge->min = le32_to_cpu(s_rge->min);
  60         d_rge->max = le32_to_cpu(s_rge->max);
  61         d_rge->step = le32_to_cpu(s_rge->step);
  62 }
  63
  64 static size_t
  65 virtio_video_parse_virtio_frame_rate(struct virtio_video_device *vvd,
  66                                      struct virtio_video_format_range *f_rate,
  67                                      void *buf)
  68 {
  69         struct virtio_video_format_range *virtio_frame_rate;
  70
  71         virtio_frame_rate = buf;
  72         virtio_video_copy_fmt_range(f_rate, virtio_frame_rate);
  73
  74         return sizeof(struct virtio_video_format_range);
  75 }
  76
  77 static size_t virtio_video_parse_virtio_frame(struct virtio_video_device *vvd,
  78                                               struct video_format_frame *frm,
  79                                               void *buf)
  80 {
  81         struct virtio_video_format_frame *virtio_frame;
  82         struct virtio_video_format_frame *frame = &frm->frame;
  83         struct virtio_video_format_range *rate;
  84         size_t idx, offset, extra_size;
  85
  86         virtio_frame = buf;
  87
  88         virtio_video_copy_fmt_range(&frame->width, &virtio_frame->width);
  89         virtio_video_copy_fmt_range(&frame->height, &virtio_frame->height);
  90
  91         frame->num_rates = le32_to_cpu(virtio_frame->num_rates);
  92         frm->frame_rates = kcalloc(frame->num_rates,
  93                                    sizeof(struct virtio_video_format_range),
  94                                    GFP_KERNEL);
  95
  96         offset = sizeof(struct virtio_video_format_frame);
  97         for (idx = 0; idx < frame->num_rates; idx++) {
  98                 rate = &frm->frame_rates[idx];
  99                 extra_size =
 100                         virtio_video_parse_virtio_frame_rate(vvd, rate,
 101                                                              buf + offset);
 102                 if (extra_size == 0) {
 103                         kfree(frm->frame_rates);
 104                         v4l2_err(&vvd->v4l2_dev,
 105                                  "failed to parse frame rate\n");
 106                         return 0;
 107                 }
 108                 offset += extra_size;
 109         }
 110
 111         return offset;
 112 }
 113
 114 static size_t virtio_video_parse_virtio_fmt(struct virtio_video_device *vvd,
 115                                             struct video_format *fmt, void *buf)
 116 {
 117         struct virtio_video_format_desc *virtio_fmt_desc;
 118         struct virtio_video_format_desc *fmt_desc;
 119         struct video_format_frame *frame;
 120         size_t idx, offset, extra_size;
 121
 122         virtio_fmt_desc = buf;
 123         fmt_desc = &fmt->desc;
 124
 125         fmt_desc->format =
 126                 virtio_video_format_to_v4l2
 127                 (le32_to_cpu(virtio_fmt_desc->format));
 128         fmt_desc->mask = le64_to_cpu(virtio_fmt_desc->mask);
 129         fmt_desc->planes_layout = le32_to_cpu(virtio_fmt_desc->planes_layout);
 130
 131         fmt_desc->num_frames = le32_to_cpu(virtio_fmt_desc->num_frames);
 132         fmt->frames = kcalloc(fmt_desc->num_frames,
 133                               sizeof(struct video_format_frame),
 134                               GFP_KERNEL);
 135
 136         offset = sizeof(struct virtio_video_format_desc);
 137         for (idx = 0; idx < fmt_desc->num_frames; idx++) {
 138                 frame = &fmt->frames[idx];
 139                 extra_size =
 140                         virtio_video_parse_virtio_frame(vvd, frame,
 141                                                         buf + offset);
 142                 if (extra_size == 0) {
 143                         kfree(fmt->frames);
 144                         v4l2_err(&vvd->v4l2_dev, "failed to parse frame\n");
 145                         return 0;
 146                 }
 147                 offset += extra_size;
 148         }
 149
 150         return offset;
 151 }
 152
 153 int virtio_video_parse_virtio_capability(struct virtio_video_device *vvd,
 154                                          void *resp_buf,
 155                                          struct list_head *ret_fmt_list,
 156                                          uint32_t *ret_num_fmts)
 157 {
 158         struct virtio_video_query_capability_resp *resp = resp_buf;
 159         struct video_format *fmt;
 160         uint32_t fmt_count;
 161         int fmt_idx;
 162         size_t offset;
 163         int ret;
 164
 165         if (!resp || ret_fmt_list == NULL || ret_num_fmts == NULL) {
 166                 v4l2_err(&vvd->v4l2_dev, "invalid arguments!\n");
 167                 return -EINVAL;
 168         }
 169
 170         if (le32_to_cpu(resp->num_descs) <= 0) {
 171                 v4l2_err(&vvd->v4l2_dev, "invalid capability response\n");
 172                 return -EINVAL;
 173         }
 174
 175         fmt_count = le32_to_cpu(resp->num_descs);
 176         offset = sizeof(struct virtio_video_query_capability_resp);
 177
 178         for (fmt_idx = 0; fmt_idx < fmt_count; fmt_idx++) {
 179                 size_t fmt_size = 0;
 180
 181                 fmt = kzalloc(sizeof(*fmt), GFP_KERNEL);
 182                 if (!fmt) {
 183                         ret = -ENOMEM;
 184                         goto alloc_err;
 185                 }
 186
 187                 fmt_size = virtio_video_parse_virtio_fmt(vvd, fmt,
 188                                                          resp_buf + offset);
 189                 if (fmt_size == 0) {
 190                         v4l2_err(&vvd->v4l2_dev, "failed to parse fmt\n");
 191                         ret = -ENOENT;
 192                         goto parse_fmt_err;
 193                 }
 194                 offset += fmt_size;
 195                 list_add(&fmt->formats_list_entry, ret_fmt_list);
 196         }
 197
 198         *ret_num_fmts = fmt_count;
 199         return 0;
 200
 201 parse_fmt_err:
 202         kfree(fmt);
 203 alloc_err:
 204         virtio_video_free_fmts(vvd);
 205         return ret;
 206 }
 207
 208 int virtio_video_parse_virtio_capabilities(struct virtio_video_device *vvd,
 209                                            void *input_buf, void *output_buf)
 210 {
 211         int ret;
 212
 213         if (input_buf) {
 214                 ret = virtio_video_parse_virtio_capability(vvd, input_buf,
 215                                                 &vvd->input_fmt_list,
 216                                                 &vvd->num_input_fmts);
 217                 if (ret) {
 218                         v4l2_err(&vvd->v4l2_dev,
 219                                  "Failed to parse input capability: %d\n",
 220                                  ret);
 221                         return ret;
 222                 }
 223         }
 224
 225         if (output_buf) {
 226                 ret = virtio_video_parse_virtio_capability(vvd, output_buf,
 227                                                  &vvd->output_fmt_list,
 228                                                  &vvd->num_output_fmts);
 229                 if (ret) {
 230                         v4l2_err(&vvd->v4l2_dev,
 231                                  "Failed to parse output capability: %d\n",
 232                                  ret);
 233                         return ret;
 234                 }
 235         }
 236
 237         return 0;
 238 }
 239
 240 void virtio_video_clean_capability(struct virtio_video_device *vvd)
 241 {
 242         virtio_video_free_fmts(vvd);
 243 }
 244
 245 static void
 246 virtio_video_free_control_fmt_data(struct video_control_fmt_data *data)
 247 {
 248         kfree(data->entries);
 249         kfree(data);
 250 }
 251
 252 static void virtio_video_free_control_formats(struct virtio_video_device *vvd)
 253 {
 254         struct video_control_format *c_fmt, *tmp;
 255
 256         list_for_each_entry_safe(c_fmt, tmp, &vvd->controls_fmt_list,
 257                                  controls_list_entry) {
 258                 list_del(&c_fmt->controls_list_entry);
 259                 virtio_video_free_control_fmt_data(c_fmt->profile);
 260                 virtio_video_free_control_fmt_data(c_fmt->level);
 261                 kfree(c_fmt);
 262         }
 263 }
 264
 265 static int virtio_video_parse_control_levels(struct virtio_video_device *vvd,
 266                                              struct video_control_format *fmt)
 267 {
 268         int idx, ret;
 269         struct virtio_video_query_control_resp *resp_buf;
 270         struct virtio_video_query_control_resp_level *l_resp_buf;
 271         struct video_control_fmt_data *level;
 272         enum virtio_video_format virtio_format;
 273         uint32_t *virtio_levels;
 274         uint32_t num_levels, mask = 0;
 275         int max = 0, min = UINT_MAX;
 276         size_t resp_size;
 277
 278         resp_size = vvd->max_resp_len;
 279
 280         virtio_format = virtio_video_v4l2_format_to_virtio(fmt->format);
 281
 282         resp_buf = kzalloc(resp_size, GFP_KERNEL);
 283         if (IS_ERR(resp_buf)) {
 284                 ret = PTR_ERR(resp_buf);
 285                 goto lvl_err;
 286         }
 287
 288         ret = virtio_video_query_control_level(vvd, resp_buf, resp_size,
 289                                                virtio_format);
 290         if (ret) {
 291                 v4l2_err(&vvd->v4l2_dev, "failed to query level\n");
 292                 goto lvl_err;
 293         }
 294
 295         l_resp_buf = (void *)((char *)resp_buf + sizeof(*resp_buf));
 296         num_levels = le32_to_cpu(l_resp_buf->num);
 297         if (num_levels == 0)
 298                 goto lvl_err;
 299
 300         fmt->level = kzalloc(sizeof(*level), GFP_KERNEL);
 301         if (!fmt->level) {
 302                 ret = -ENOMEM;
 303                 goto lvl_err;
 304         }
 305
 306         level = fmt->level;
 307         level->entries = kcalloc(num_levels, sizeof(uint32_t), GFP_KERNEL);
 308         if (!level->entries) {
 309                 kfree(fmt->level);
 310                 ret = -ENOMEM;
 311                 goto lvl_err;
 312         }
 313
 314         virtio_levels = (void *)((char *)l_resp_buf + sizeof(*l_resp_buf));
 315
 316         for (idx = 0; idx < num_levels; idx++) {
 317                 level->entries[idx] =
 318                         virtio_video_level_to_v4l2
 319                         (le32_to_cpu(virtio_levels[idx]));
 320
 321                 mask = mask | (1 << level->entries[idx]);
 322                 if (level->entries[idx] > max)
 323                         max = level->entries[idx];
 324                 if (level->entries[idx] < min)
 325                         min = level->entries[idx];
 326         }
 327         level->min = min;
 328         level->max = max;
 329         level->num = num_levels;
 330         level->skip_mask = ~mask;
 331
 332 lvl_err:
 333         kfree(resp_buf);
 334
 335         return ret;
 336 }
 337
 338 static int virtio_video_parse_control_profiles(struct virtio_video_device *vvd,
 339                                                struct video_control_format *fmt)
 340 {
 341         int idx, ret;
 342         struct virtio_video_query_control_resp *resp_buf;
 343         struct virtio_video_query_control_resp_profile *p_resp_buf;
 344         struct video_control_fmt_data *profile;
 345         uint32_t virtio_format, num_profiles, mask = 0;
 346         uint32_t *virtio_profiles;
 347         int max = 0, min = UINT_MAX;
 348         size_t resp_size;
 349
 350         resp_size = vvd->max_resp_len;
 351         virtio_format = virtio_video_v4l2_format_to_virtio(fmt->format);
 352         resp_buf = kzalloc(resp_size, GFP_KERNEL);
 353         if (IS_ERR(resp_buf)) {
 354                 ret = PTR_ERR(resp_buf);
 355                 goto prf_err;
 356         }
 357
 358         ret = virtio_video_query_control_profile(vvd, resp_buf, resp_size,
 359                                                  virtio_format);
 360         if (ret) {
 361                 v4l2_err(&vvd->v4l2_dev, "failed to query profile\n");
 362                 goto prf_err;
 363         }
 364
 365         p_resp_buf = (void *)((char *)resp_buf + sizeof(*resp_buf));
 366         num_profiles = le32_to_cpu(p_resp_buf->num);
 367         if (num_profiles == 0)
 368                 goto prf_err;
 369
 370         fmt->profile = kzalloc(sizeof(*profile), GFP_KERNEL);
 371         if (!fmt->profile) {
 372                 ret = -ENOMEM;
 373                 goto prf_err;
 374         }
 375
 376         profile = fmt->profile;
 377         profile->entries = kcalloc(num_profiles, sizeof(uint32_t), GFP_KERNEL);
 378         if (!profile->entries) {
 379                 kfree(fmt->profile);
 380                 ret = -ENOMEM;
 381                 goto prf_err;
 382         }
 383
 384         virtio_profiles = (void *)((char *)p_resp_buf + sizeof(*p_resp_buf));
 385
 386         for (idx = 0; idx < num_profiles; idx++) {
 387                 profile->entries[idx] =
 388                         virtio_video_profile_to_v4l2
 389                         (le32_to_cpu(virtio_profiles[idx]));
 390
 391                 mask = mask | (1 << profile->entries[idx]);
 392                 if (profile->entries[idx] > max)
 393                         max = profile->entries[idx];
 394                 if (profile->entries[idx] < min)
 395                         min = profile->entries[idx];
 396         }
 397         profile->min = min;
 398         profile->max = max;
 399         profile->num = num_profiles;
 400         profile->skip_mask = ~mask;
 401
 402 prf_err:
 403         kfree(resp_buf);
 404
 405         return ret;
 406 }
 407
 408 int virtio_video_parse_virtio_control(struct virtio_video_device *vvd)
 409 {
 410         struct video_format *fmt;
 411         struct video_control_format *c_fmt;
 412         uint32_t virtio_format;
 413         int ret;
 414
 415         list_for_each_entry(fmt, &vvd->output_fmt_list, formats_list_entry) {
 416                 virtio_format =
 417                         virtio_video_v4l2_format_to_virtio(fmt->desc.format);
 418                 if (virtio_format < VIRTIO_VIDEO_FORMAT_CODED_MIN ||
 419                     virtio_format > VIRTIO_VIDEO_FORMAT_CODED_MAX)
 420                         continue;
 421
 422                 c_fmt = kzalloc(sizeof(*c_fmt), GFP_KERNEL);
 423                 if (!c_fmt) {
 424                         ret = -ENOMEM;
 425                         goto parse_ctrl_alloc_err;
 426                 }
 427
 428                 c_fmt->format = fmt->desc.format;
 429
 430                 ret = virtio_video_parse_control_profiles(vvd, c_fmt);
 431                 if (ret) {
 432                         v4l2_err(&vvd->v4l2_dev,
 433                                  "failed to parse control profile\n");
 434                         goto parse_ctrl_prf_err;
 435                 }
 436
 437                 ret = virtio_video_parse_control_levels(vvd, c_fmt);
 438                 if (ret) {
 439                         v4l2_err(&vvd->v4l2_dev,
 440                                  "failed to parse control level\n");
 441                         goto parse_ctrl_lvl_err;
 442                 }
 443                 list_add(&c_fmt->controls_list_entry, &vvd->controls_fmt_list);
 444         }
 445         return 0;
 446
 447 parse_ctrl_lvl_err:
 448         virtio_video_free_control_fmt_data(c_fmt->profile);
 449 parse_ctrl_prf_err:
 450         kfree(c_fmt);
 451 parse_ctrl_alloc_err:
 452         virtio_video_free_control_formats(vvd);
 453
 454         return ret;
 455 }
 456
 457 void virtio_video_clean_control(struct virtio_video_device *vvd)
 458 {
 459         virtio_video_free_control_formats(vvd);
 460 }