drivers/media/v4l2-core/v4l2-subdev.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * V4L2 sub-device
   4  *
   5  * Copyright (C) 2010 Nokia Corporation
   6  *
   7  * Contact: Laurent Pinchart <[email protected]>
   8  *          Sakari Ailus <[email protected]>
   9  */
  10
  11 #include <linux/export.h>
  12 #include <linux/ioctl.h>
  13 #include <linux/leds.h>
  14 #include <linux/mm.h>
  15 #include <linux/module.h>
  16 #include <linux/overflow.h>
  17 #include <linux/slab.h>
  18 #include <linux/string.h>
  19 #include <linux/types.h>
  20 #include <linux/version.h>
  21 #include <linux/videodev2.h>
  22
  23 #include <media/v4l2-ctrls.h>
  24 #include <media/v4l2-device.h>
  25 #include <media/v4l2-event.h>
  26 #include <media/v4l2-fh.h>
  27 #include <media/v4l2-ioctl.h>
  28
  29 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
  30 /*
  31  * The Streams API is an experimental feature. To use the Streams API, set
  32  * 'v4l2_subdev_enable_streams_api' to 1 below.
  33  */
  34
  35 static bool v4l2_subdev_enable_streams_api;
  36 #endif
  37
  38 /*
  39  * Maximum stream ID is 63 for now, as we use u64 bitmask to represent a set
  40  * of streams.
  41  *
  42  * Note that V4L2_FRAME_DESC_ENTRY_MAX is related: V4L2_FRAME_DESC_ENTRY_MAX
  43  * restricts the total number of streams in a pad, although the stream ID is
  44  * not restricted.
  45  */
  46 #define V4L2_SUBDEV_MAX_STREAM_ID 63
  47
  48 #include "v4l2-subdev-priv.h"
  49
  50 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
  51 static int subdev_fh_init(struct v4l2_subdev_fh *fh, struct v4l2_subdev *sd)
  52 {
  53         struct v4l2_subdev_state *state;
  54         static struct lock_class_key key;
  55
  56         state = __v4l2_subdev_state_alloc(sd, "fh->state->lock", &key);
  57         if (IS_ERR(state))
  58                 return PTR_ERR(state);
  59
  60         fh->state = state;
  61
  62         return 0;
  63 }
  64
  65 static void subdev_fh_free(struct v4l2_subdev_fh *fh)
  66 {
  67         __v4l2_subdev_state_free(fh->state);
  68         fh->state = NULL;
  69 }
  70
  71 static int subdev_open(struct file *file)
  72 {
  73         struct video_device *vdev = video_devdata(file);
  74         struct v4l2_subdev *sd = vdev_to_v4l2_subdev(vdev);
  75         struct v4l2_subdev_fh *subdev_fh;
  76         int ret;
  77
  78         subdev_fh = kzalloc(sizeof(*subdev_fh), GFP_KERNEL);
  79         if (subdev_fh == NULL)
  80                 return -ENOMEM;
  81
  82         ret = subdev_fh_init(subdev_fh, sd);
  83         if (ret) {
  84                 kfree(subdev_fh);
  85                 return ret;
  86         }
  87
  88         v4l2_fh_init(&subdev_fh->vfh, vdev);
  89         v4l2_fh_add(&subdev_fh->vfh);
  90         file->private_data = &subdev_fh->vfh;
  91
  92         if (sd->v4l2_dev->mdev && sd->entity.graph_obj.mdev->dev) {
  93                 struct module *owner;
  94
  95                 owner = sd->entity.graph_obj.mdev->dev->driver->owner;
  96                 if (!try_module_get(owner)) {
  97                         ret = -EBUSY;
  98                         goto err;
  99                 }
 100                 subdev_fh->owner = owner;
 101         }
 102
 103         if (sd->internal_ops && sd->internal_ops->open) {
 104                 ret = sd->internal_ops->open(sd, subdev_fh);
 105                 if (ret < 0)
 106                         goto err;
 107         }
 108
 109         return 0;
 110
 111 err:
 112         module_put(subdev_fh->owner);
 113         v4l2_fh_del(&subdev_fh->vfh);
 114         v4l2_fh_exit(&subdev_fh->vfh);
 115         subdev_fh_free(subdev_fh);
 116         kfree(subdev_fh);
 117
 118         return ret;
 119 }
 120
 121 static int subdev_close(struct file *file)
 122 {
 123         struct video_device *vdev = video_devdata(file);
 124         struct v4l2_subdev *sd = vdev_to_v4l2_subdev(vdev);
 125         struct v4l2_fh *vfh = file->private_data;
 126         struct v4l2_subdev_fh *subdev_fh = to_v4l2_subdev_fh(vfh);
 127
 128         if (sd->internal_ops && sd->internal_ops->close)
 129                 sd->internal_ops->close(sd, subdev_fh);
 130         module_put(subdev_fh->owner);
 131         v4l2_fh_del(vfh);
 132         v4l2_fh_exit(vfh);
 133         subdev_fh_free(subdev_fh);
 134         kfree(subdev_fh);
 135         file->private_data = NULL;
 136
 137         return 0;
 138 }
 139 #else /* CONFIG_VIDEO_V4L2_SUBDEV_API */
 140 static int subdev_open(struct file *file)
 141 {
 142         return -ENODEV;
 143 }
 144
 145 static int subdev_close(struct file *file)
 146 {
 147         return -ENODEV;
 148 }
 149 #endif /* CONFIG_VIDEO_V4L2_SUBDEV_API */
 150
 151 static inline int check_which(u32 which)
 152 {
 153         if (which != V4L2_SUBDEV_FORMAT_TRY &&
 154             which != V4L2_SUBDEV_FORMAT_ACTIVE)
 155                 return -EINVAL;
 156
 157         return 0;
 158 }
 159
 160 static inline int check_pad(struct v4l2_subdev *sd, u32 pad)
 161 {
 162 #if defined(CONFIG_MEDIA_CONTROLLER)
 163         if (sd->entity.num_pads) {
 164                 if (pad >= sd->entity.num_pads)
 165                         return -EINVAL;
 166                 return 0;
 167         }
 168 #endif
 169         /* allow pad 0 on subdevices not registered as media entities */
 170         if (pad > 0)
 171                 return -EINVAL;
 172         return 0;
 173 }
 174
 175 static int check_state(struct v4l2_subdev *sd, struct v4l2_subdev_state *state,
 176                        u32 which, u32 pad, u32 stream)
 177 {
 178         if (sd->flags & V4L2_SUBDEV_FL_STREAMS) {
 179 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
 180                 if (!v4l2_subdev_state_get_stream_format(state, pad, stream))
 181                         return -EINVAL;
 182                 return 0;
 183 #else
 184                 return -EINVAL;
 185 #endif
 186         }
 187
 188         if (stream != 0)
 189                 return -EINVAL;
 190
 191         if (which == V4L2_SUBDEV_FORMAT_TRY && (!state || !state->pads))
 192                 return -EINVAL;
 193
 194         return 0;
 195 }
 196
 197 static inline int check_format(struct v4l2_subdev *sd,
 198                                struct v4l2_subdev_state *state,
 199                                struct v4l2_subdev_format *format)
 200 {
 201         if (!format)
 202                 return -EINVAL;
 203
 204         return check_which(format->which) ? : check_pad(sd, format->pad) ? :
 205                check_state(sd, state, format->which, format->pad, format->stream);
 206 }
 207
 208 static int call_get_fmt(struct v4l2_subdev *sd,
 209                         struct v4l2_subdev_state *state,
 210                         struct v4l2_subdev_format *format)
 211 {
 212         return check_format(sd, state, format) ? :
 213                sd->ops->pad->get_fmt(sd, state, format);
 214 }
 215
 216 static int call_set_fmt(struct v4l2_subdev *sd,
 217                         struct v4l2_subdev_state *state,
 218                         struct v4l2_subdev_format *format)
 219 {
 220         return check_format(sd, state, format) ? :
 221                sd->ops->pad->set_fmt(sd, state, format);
 222 }
 223
 224 static int call_enum_mbus_code(struct v4l2_subdev *sd,
 225                                struct v4l2_subdev_state *state,
 226                                struct v4l2_subdev_mbus_code_enum *code)
 227 {
 228         if (!code)
 229                 return -EINVAL;
 230
 231         return check_which(code->which) ? : check_pad(sd, code->pad) ? :
 232                check_state(sd, state, code->which, code->pad, code->stream) ? :
 233                sd->ops->pad->enum_mbus_code(sd, state, code);
 234 }
 235
 236 static int call_enum_frame_size(struct v4l2_subdev *sd,
 237                                 struct v4l2_subdev_state *state,
 238                                 struct v4l2_subdev_frame_size_enum *fse)
 239 {
 240         if (!fse)
 241                 return -EINVAL;
 242
 243         return check_which(fse->which) ? : check_pad(sd, fse->pad) ? :
 244                check_state(sd, state, fse->which, fse->pad, fse->stream) ? :
 245                sd->ops->pad->enum_frame_size(sd, state, fse);
 246 }
 247
 248 static inline int check_frame_interval(struct v4l2_subdev *sd,
 249                                        struct v4l2_subdev_frame_interval *fi)
 250 {
 251         if (!fi)
 252                 return -EINVAL;
 253
 254         return check_pad(sd, fi->pad);
 255 }
 256
 257 static int call_g_frame_interval(struct v4l2_subdev *sd,
 258                                  struct v4l2_subdev_frame_interval *fi)
 259 {
 260         return check_frame_interval(sd, fi) ? :
 261                sd->ops->video->g_frame_interval(sd, fi);
 262 }
 263
 264 static int call_s_frame_interval(struct v4l2_subdev *sd,
 265                                  struct v4l2_subdev_frame_interval *fi)
 266 {
 267         return check_frame_interval(sd, fi) ? :
 268                sd->ops->video->s_frame_interval(sd, fi);
 269 }
 270
 271 static int call_enum_frame_interval(struct v4l2_subdev *sd,
 272                                     struct v4l2_subdev_state *state,
 273                                     struct v4l2_subdev_frame_interval_enum *fie)
 274 {
 275         if (!fie)
 276                 return -EINVAL;
 277
 278         return check_which(fie->which) ? : check_pad(sd, fie->pad) ? :
 279                check_state(sd, state, fie->which, fie->pad, fie->stream) ? :
 280                sd->ops->pad->enum_frame_interval(sd, state, fie);
 281 }
 282
 283 static inline int check_selection(struct v4l2_subdev *sd,
 284                                   struct v4l2_subdev_state *state,
 285                                   struct v4l2_subdev_selection *sel)
 286 {
 287         if (!sel)
 288                 return -EINVAL;
 289
 290         return check_which(sel->which) ? : check_pad(sd, sel->pad) ? :
 291                check_state(sd, state, sel->which, sel->pad, sel->stream);
 292 }
 293
 294 static int call_get_selection(struct v4l2_subdev *sd,
 295                               struct v4l2_subdev_state *state,
 296                               struct v4l2_subdev_selection *sel)
 297 {
 298         return check_selection(sd, state, sel) ? :
 299                sd->ops->pad->get_selection(sd, state, sel);
 300 }
 301
 302 static int call_set_selection(struct v4l2_subdev *sd,
 303                               struct v4l2_subdev_state *state,
 304                               struct v4l2_subdev_selection *sel)
 305 {
 306         return check_selection(sd, state, sel) ? :
 307                sd->ops->pad->set_selection(sd, state, sel);
 308 }
 309
 310 static int call_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 311                                struct v4l2_mbus_frame_desc *fd)
 312 {
 313         unsigned int i;
 314         int ret;
 315
 316         memset(fd, 0, sizeof(*fd));
 317
 318         ret = sd->ops->pad->get_frame_desc(sd, pad, fd);
 319         if (ret)
 320                 return ret;
 321
 322         dev_dbg(sd->dev, "Frame descriptor on pad %u, type %s\n", pad,
 323                 fd->type == V4L2_MBUS_FRAME_DESC_TYPE_PARALLEL ? "parallel" :
 324                 fd->type == V4L2_MBUS_FRAME_DESC_TYPE_CSI2 ? "CSI-2" :
 325                 "unknown");
 326
 327         for (i = 0; i < fd->num_entries; i++) {
 328                 struct v4l2_mbus_frame_desc_entry *entry = &fd->entry[i];
 329                 char buf[20] = "";
 330
 331                 if (fd->type == V4L2_MBUS_FRAME_DESC_TYPE_CSI2)
 332                         WARN_ON(snprintf(buf, sizeof(buf),
 333                                          ", vc %u, dt 0x%02x",
 334                                          entry->bus.csi2.vc,
 335                                          entry->bus.csi2.dt) >= sizeof(buf));
 336
 337                 dev_dbg(sd->dev,
 338                         "\tstream %u, code 0x%04x, length %u, flags 0x%04x%s\n",
 339                         entry->stream, entry->pixelcode, entry->length,
 340                         entry->flags, buf);
 341         }
 342
 343         return 0;
 344 }
 345
 346 static inline int check_edid(struct v4l2_subdev *sd,
 347                              struct v4l2_subdev_edid *edid)
 348 {
 349         if (!edid)
 350                 return -EINVAL;
 351
 352         if (edid->blocks && edid->edid == NULL)
 353                 return -EINVAL;
 354
 355         return check_pad(sd, edid->pad);
 356 }
 357
 358 static int call_get_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
 359 {
 360         return check_edid(sd, edid) ? : sd->ops->pad->get_edid(sd, edid);
 361 }
 362
 363 static int call_set_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
 364 {
 365         return check_edid(sd, edid) ? : sd->ops->pad->set_edid(sd, edid);
 366 }
 367
 368 static int call_dv_timings_cap(struct v4l2_subdev *sd,
 369                                struct v4l2_dv_timings_cap *cap)
 370 {
 371         if (!cap)
 372                 return -EINVAL;
 373
 374         return check_pad(sd, cap->pad) ? :
 375                sd->ops->pad->dv_timings_cap(sd, cap);
 376 }
 377
 378 static int call_enum_dv_timings(struct v4l2_subdev *sd,
 379                                 struct v4l2_enum_dv_timings *dvt)
 380 {
 381         if (!dvt)
 382                 return -EINVAL;
 383
 384         return check_pad(sd, dvt->pad) ? :
 385                sd->ops->pad->enum_dv_timings(sd, dvt);
 386 }
 387
 388 static int call_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
 389                                 struct v4l2_mbus_config *config)
 390 {
 391         return check_pad(sd, pad) ? :
 392                sd->ops->pad->get_mbus_config(sd, pad, config);
 393 }
 394
 395 static int call_s_stream(struct v4l2_subdev *sd, int enable)
 396 {
 397         int ret;
 398
 399         /*
 400          * The .s_stream() operation must never be called to start or stop an
 401          * already started or stopped subdev. Catch offenders but don't return
 402          * an error yet to avoid regressions.
 403          *
 404          * As .s_stream() is mutually exclusive with the .enable_streams() and
 405          * .disable_streams() operation, we can use the enabled_streams field
 406          * to store the subdev streaming state.
 407          */
 408         if (WARN_ON(!!sd->enabled_streams == !!enable))
 409                 return 0;
 410
 411 #if IS_REACHABLE(CONFIG_LEDS_CLASS)
 412         if (!IS_ERR_OR_NULL(sd->privacy_led)) {
 413                 if (enable)
 414                         led_set_brightness(sd->privacy_led,
 415                                            sd->privacy_led->max_brightness);
 416                 else
 417                         led_set_brightness(sd->privacy_led, 0);
 418         }
 419 #endif
 420         ret = sd->ops->video->s_stream(sd, enable);
 421
 422         if (!enable && ret < 0) {
 423                 dev_warn(sd->dev, "disabling streaming failed (%d)\n", ret);
 424                 ret = 0;
 425         }
 426
 427         if (!ret)
 428                 sd->enabled_streams = enable ? BIT(0) : 0;
 429
 430         return ret;
 431 }
 432
 433 #ifdef CONFIG_MEDIA_CONTROLLER
 434 /*
 435  * Create state-management wrapper for pad ops dealing with subdev state. The
 436  * wrapper handles the case where the caller does not provide the called
 437  * subdev's state. This should be removed when all the callers are fixed.
 438  */
 439 #define DEFINE_STATE_WRAPPER(f, arg_type)                                  \
 440         static int call_##f##_state(struct v4l2_subdev *sd,                \
 441                                     struct v4l2_subdev_state *_state,      \
 442                                     arg_type *arg)                         \
 443         {                                                                  \
 444                 struct v4l2_subdev_state *state = _state;                  \
 445                 int ret;                                                   \
 446                 if (!_state)                                               \
 447                         state = v4l2_subdev_lock_and_get_active_state(sd); \
 448                 ret = call_##f(sd, state, arg);                            \
 449                 if (!_state && state)                                      \
 450                         v4l2_subdev_unlock_state(state);                   \
 451                 return ret;                                                \
 452         }
 453
 454 #else /* CONFIG_MEDIA_CONTROLLER */
 455
 456 #define DEFINE_STATE_WRAPPER(f, arg_type)                            \
 457         static int call_##f##_state(struct v4l2_subdev *sd,          \
 458                                     struct v4l2_subdev_state *state, \
 459                                     arg_type *arg)                   \
 460         {                                                            \
 461                 return call_##f(sd, state, arg);                     \
 462         }
 463
 464 #endif /* CONFIG_MEDIA_CONTROLLER */
 465
 466 DEFINE_STATE_WRAPPER(get_fmt, struct v4l2_subdev_format);
 467 DEFINE_STATE_WRAPPER(set_fmt, struct v4l2_subdev_format);
 468 DEFINE_STATE_WRAPPER(enum_mbus_code, struct v4l2_subdev_mbus_code_enum);
 469 DEFINE_STATE_WRAPPER(enum_frame_size, struct v4l2_subdev_frame_size_enum);
 470 DEFINE_STATE_WRAPPER(enum_frame_interval, struct v4l2_subdev_frame_interval_enum);
 471 DEFINE_STATE_WRAPPER(get_selection, struct v4l2_subdev_selection);
 472 DEFINE_STATE_WRAPPER(set_selection, struct v4l2_subdev_selection);
 473
 474 static const struct v4l2_subdev_pad_ops v4l2_subdev_call_pad_wrappers = {
 475         .get_fmt                = call_get_fmt_state,
 476         .set_fmt                = call_set_fmt_state,
 477         .enum_mbus_code         = call_enum_mbus_code_state,
 478         .enum_frame_size        = call_enum_frame_size_state,
 479         .enum_frame_interval    = call_enum_frame_interval_state,
 480         .get_selection          = call_get_selection_state,
 481         .set_selection          = call_set_selection_state,
 482         .get_edid               = call_get_edid,
 483         .set_edid               = call_set_edid,
 484         .dv_timings_cap         = call_dv_timings_cap,
 485         .enum_dv_timings        = call_enum_dv_timings,
 486         .get_frame_desc         = call_get_frame_desc,
 487         .get_mbus_config        = call_get_mbus_config,
 488 };
 489
 490 static const struct v4l2_subdev_video_ops v4l2_subdev_call_video_wrappers = {
 491         .g_frame_interval       = call_g_frame_interval,
 492         .s_frame_interval       = call_s_frame_interval,
 493         .s_stream               = call_s_stream,
 494 };
 495
 496 const struct v4l2_subdev_ops v4l2_subdev_call_wrappers = {
 497         .pad    = &v4l2_subdev_call_pad_wrappers,
 498         .video  = &v4l2_subdev_call_video_wrappers,
 499 };
 500 EXPORT_SYMBOL(v4l2_subdev_call_wrappers);
 501
 502 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
 503
 504 static struct v4l2_subdev_state *
 505 subdev_ioctl_get_state(struct v4l2_subdev *sd, struct v4l2_subdev_fh *subdev_fh,
 506                        unsigned int cmd, void *arg)
 507 {
 508         u32 which;
 509
 510         switch (cmd) {
 511         default:
 512                 return NULL;
 513         case VIDIOC_SUBDEV_G_FMT:
 514         case VIDIOC_SUBDEV_S_FMT:
 515                 which = ((struct v4l2_subdev_format *)arg)->which;
 516                 break;
 517         case VIDIOC_SUBDEV_G_CROP:
 518         case VIDIOC_SUBDEV_S_CROP:
 519                 which = ((struct v4l2_subdev_crop *)arg)->which;
 520                 break;
 521         case VIDIOC_SUBDEV_ENUM_MBUS_CODE:
 522                 which = ((struct v4l2_subdev_mbus_code_enum *)arg)->which;
 523                 break;
 524         case VIDIOC_SUBDEV_ENUM_FRAME_SIZE:
 525                 which = ((struct v4l2_subdev_frame_size_enum *)arg)->which;
 526                 break;
 527         case VIDIOC_SUBDEV_ENUM_FRAME_INTERVAL:
 528                 which = ((struct v4l2_subdev_frame_interval_enum *)arg)->which;
 529                 break;
 530         case VIDIOC_SUBDEV_G_SELECTION:
 531         case VIDIOC_SUBDEV_S_SELECTION:
 532                 which = ((struct v4l2_subdev_selection *)arg)->which;
 533                 break;
 534         case VIDIOC_SUBDEV_G_ROUTING:
 535         case VIDIOC_SUBDEV_S_ROUTING:
 536                 which = ((struct v4l2_subdev_routing *)arg)->which;
 537                 break;
 538         }
 539
 540         return which == V4L2_SUBDEV_FORMAT_TRY ?
 541                              subdev_fh->state :
 542                              v4l2_subdev_get_unlocked_active_state(sd);
 543 }
 544
 545 static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 546                             struct v4l2_subdev_state *state)
 547 {
 548         struct video_device *vdev = video_devdata(file);
 549         struct v4l2_subdev *sd = vdev_to_v4l2_subdev(vdev);
 550         struct v4l2_fh *vfh = file->private_data;
 551         struct v4l2_subdev_fh *subdev_fh = to_v4l2_subdev_fh(vfh);
 552         bool ro_subdev = test_bit(V4L2_FL_SUBDEV_RO_DEVNODE, &vdev->flags);
 553         bool streams_subdev = sd->flags & V4L2_SUBDEV_FL_STREAMS;
 554         bool client_supports_streams = subdev_fh->client_caps &
 555                                        V4L2_SUBDEV_CLIENT_CAP_STREAMS;
 556         int rval;
 557
 558         /*
 559          * If the streams API is not enabled, remove V4L2_SUBDEV_CAP_STREAMS.
 560          * Remove this when the API is no longer experimental.
 561          */
 562         if (!v4l2_subdev_enable_streams_api)
 563                 streams_subdev = false;
 564
 565         switch (cmd) {
 566         case VIDIOC_SUBDEV_QUERYCAP: {
 567                 struct v4l2_subdev_capability *cap = arg;
 568
 569                 memset(cap->reserved, 0, sizeof(cap->reserved));
 570                 cap->version = LINUX_VERSION_CODE;
 571                 cap->capabilities =
 572                         (ro_subdev ? V4L2_SUBDEV_CAP_RO_SUBDEV : 0) |
 573                         (streams_subdev ? V4L2_SUBDEV_CAP_STREAMS : 0);
 574
 575                 return 0;
 576         }
 577
 578         case VIDIOC_QUERYCTRL:
 579                 /*
 580                  * TODO: this really should be folded into v4l2_queryctrl (this
 581                  * currently returns -EINVAL for NULL control handlers).
 582                  * However, v4l2_queryctrl() is still called directly by
 583                  * drivers as well and until that has been addressed I believe
 584                  * it is safer to do the check here. The same is true for the
 585                  * other control ioctls below.
 586                  */
 587                 if (!vfh->ctrl_handler)
 588                         return -ENOTTY;
 589                 return v4l2_queryctrl(vfh->ctrl_handler, arg);
 590
 591         case VIDIOC_QUERY_EXT_CTRL:
 592                 if (!vfh->ctrl_handler)
 593                         return -ENOTTY;
 594                 return v4l2_query_ext_ctrl(vfh->ctrl_handler, arg);
 595
 596         case VIDIOC_QUERYMENU:
 597                 if (!vfh->ctrl_handler)
 598                         return -ENOTTY;
 599                 return v4l2_querymenu(vfh->ctrl_handler, arg);
 600
 601         case VIDIOC_G_CTRL:
 602                 if (!vfh->ctrl_handler)
 603                         return -ENOTTY;
 604                 return v4l2_g_ctrl(vfh->ctrl_handler, arg);
 605
 606         case VIDIOC_S_CTRL:
 607                 if (!vfh->ctrl_handler)
 608                         return -ENOTTY;
 609                 return v4l2_s_ctrl(vfh, vfh->ctrl_handler, arg);
 610
 611         case VIDIOC_G_EXT_CTRLS:
 612                 if (!vfh->ctrl_handler)
 613                         return -ENOTTY;
 614                 return v4l2_g_ext_ctrls(vfh->ctrl_handler,
 615                                         vdev, sd->v4l2_dev->mdev, arg);
 616
 617         case VIDIOC_S_EXT_CTRLS:
 618                 if (!vfh->ctrl_handler)
 619                         return -ENOTTY;
 620                 return v4l2_s_ext_ctrls(vfh, vfh->ctrl_handler,
 621                                         vdev, sd->v4l2_dev->mdev, arg);
 622
 623         case VIDIOC_TRY_EXT_CTRLS:
 624                 if (!vfh->ctrl_handler)
 625                         return -ENOTTY;
 626                 return v4l2_try_ext_ctrls(vfh->ctrl_handler,
 627                                           vdev, sd->v4l2_dev->mdev, arg);
 628
 629         case VIDIOC_DQEVENT:
 630                 if (!(sd->flags & V4L2_SUBDEV_FL_HAS_EVENTS))
 631                         return -ENOIOCTLCMD;
 632
 633                 return v4l2_event_dequeue(vfh, arg, file->f_flags & O_NONBLOCK);
 634
 635         case VIDIOC_SUBSCRIBE_EVENT:
 636                 return v4l2_subdev_call(sd, core, subscribe_event, vfh, arg);
 637
 638         case VIDIOC_UNSUBSCRIBE_EVENT:
 639                 return v4l2_subdev_call(sd, core, unsubscribe_event, vfh, arg);
 640
 641 #ifdef CONFIG_VIDEO_ADV_DEBUG
 642         case VIDIOC_DBG_G_REGISTER:
 643         {
 644                 struct v4l2_dbg_register *p = arg;
 645
 646                 if (!capable(CAP_SYS_ADMIN))
 647                         return -EPERM;
 648                 return v4l2_subdev_call(sd, core, g_register, p);
 649         }
 650         case VIDIOC_DBG_S_REGISTER:
 651         {
 652                 struct v4l2_dbg_register *p = arg;
 653
 654                 if (!capable(CAP_SYS_ADMIN))
 655                         return -EPERM;
 656                 return v4l2_subdev_call(sd, core, s_register, p);
 657         }
 658         case VIDIOC_DBG_G_CHIP_INFO:
 659         {
 660                 struct v4l2_dbg_chip_info *p = arg;
 661
 662                 if (p->match.type != V4L2_CHIP_MATCH_SUBDEV || p->match.addr)
 663                         return -EINVAL;
 664                 if (sd->ops->core && sd->ops->core->s_register)
 665                         p->flags |= V4L2_CHIP_FL_WRITABLE;
 666                 if (sd->ops->core && sd->ops->core->g_register)
 667                         p->flags |= V4L2_CHIP_FL_READABLE;
 668                 strscpy(p->name, sd->name, sizeof(p->name));
 669                 return 0;
 670         }
 671 #endif
 672
 673         case VIDIOC_LOG_STATUS: {
 674                 int ret;
 675
 676                 pr_info("%s: =================  START STATUS  =================\n",
 677                         sd->name);
 678                 ret = v4l2_subdev_call(sd, core, log_status);
 679                 pr_info("%s: ==================  END STATUS  ==================\n",
 680                         sd->name);
 681                 return ret;
 682         }
 683
 684         case VIDIOC_SUBDEV_G_FMT: {
 685                 struct v4l2_subdev_format *format = arg;
 686
 687                 if (!client_supports_streams)
 688                         format->stream = 0;
 689
 690                 memset(format->reserved, 0, sizeof(format->reserved));
 691                 memset(format->format.reserved, 0, sizeof(format->format.reserved));
 692                 return v4l2_subdev_call(sd, pad, get_fmt, state, format);
 693         }
 694
 695         case VIDIOC_SUBDEV_S_FMT: {
 696                 struct v4l2_subdev_format *format = arg;
 697
 698                 if (format->which != V4L2_SUBDEV_FORMAT_TRY && ro_subdev)
 699                         return -EPERM;
 700
 701                 if (!client_supports_streams)
 702                         format->stream = 0;
 703
 704                 memset(format->reserved, 0, sizeof(format->reserved));
 705                 memset(format->format.reserved, 0, sizeof(format->format.reserved));
 706                 return v4l2_subdev_call(sd, pad, set_fmt, state, format);
 707         }
 708
 709         case VIDIOC_SUBDEV_G_CROP: {
 710                 struct v4l2_subdev_crop *crop = arg;
 711                 struct v4l2_subdev_selection sel;
 712
 713                 if (!client_supports_streams)
 714                         crop->stream = 0;
 715
 716                 memset(crop->reserved, 0, sizeof(crop->reserved));
 717                 memset(&sel, 0, sizeof(sel));
 718                 sel.which = crop->which;
 719                 sel.pad = crop->pad;
 720                 sel.target = V4L2_SEL_TGT_CROP;
 721
 722                 rval = v4l2_subdev_call(
 723                         sd, pad, get_selection, state, &sel);
 724
 725                 crop->rect = sel.r;
 726
 727                 return rval;
 728         }
 729
 730         case VIDIOC_SUBDEV_S_CROP: {
 731                 struct v4l2_subdev_crop *crop = arg;
 732                 struct v4l2_subdev_selection sel;
 733
 734                 if (crop->which != V4L2_SUBDEV_FORMAT_TRY && ro_subdev)
 735                         return -EPERM;
 736
 737                 if (!client_supports_streams)
 738                         crop->stream = 0;
 739
 740                 memset(crop->reserved, 0, sizeof(crop->reserved));
 741                 memset(&sel, 0, sizeof(sel));
 742                 sel.which = crop->which;
 743                 sel.pad = crop->pad;
 744                 sel.target = V4L2_SEL_TGT_CROP;
 745                 sel.r = crop->rect;
 746
 747                 rval = v4l2_subdev_call(
 748                         sd, pad, set_selection, state, &sel);
 749
 750                 crop->rect = sel.r;
 751
 752                 return rval;
 753         }
 754
 755         case VIDIOC_SUBDEV_ENUM_MBUS_CODE: {
 756                 struct v4l2_subdev_mbus_code_enum *code = arg;
 757
 758                 if (!client_supports_streams)
 759                         code->stream = 0;
 760
 761                 memset(code->reserved, 0, sizeof(code->reserved));
 762                 return v4l2_subdev_call(sd, pad, enum_mbus_code, state,
 763                                         code);
 764         }
 765
 766         case VIDIOC_SUBDEV_ENUM_FRAME_SIZE: {
 767                 struct v4l2_subdev_frame_size_enum *fse = arg;
 768
 769                 if (!client_supports_streams)
 770                         fse->stream = 0;
 771
 772                 memset(fse->reserved, 0, sizeof(fse->reserved));
 773                 return v4l2_subdev_call(sd, pad, enum_frame_size, state,
 774                                         fse);
 775         }
 776
 777         case VIDIOC_SUBDEV_G_FRAME_INTERVAL: {
 778                 struct v4l2_subdev_frame_interval *fi = arg;
 779
 780                 if (!client_supports_streams)
 781                         fi->stream = 0;
 782
 783                 memset(fi->reserved, 0, sizeof(fi->reserved));
 784                 return v4l2_subdev_call(sd, video, g_frame_interval, arg);
 785         }
 786
 787         case VIDIOC_SUBDEV_S_FRAME_INTERVAL: {
 788                 struct v4l2_subdev_frame_interval *fi = arg;
 789
 790                 if (ro_subdev)
 791                         return -EPERM;
 792
 793                 if (!client_supports_streams)
 794                         fi->stream = 0;
 795
 796                 memset(fi->reserved, 0, sizeof(fi->reserved));
 797                 return v4l2_subdev_call(sd, video, s_frame_interval, arg);
 798         }
 799
 800         case VIDIOC_SUBDEV_ENUM_FRAME_INTERVAL: {
 801                 struct v4l2_subdev_frame_interval_enum *fie = arg;
 802
 803                 if (!client_supports_streams)
 804                         fie->stream = 0;
 805
 806                 memset(fie->reserved, 0, sizeof(fie->reserved));
 807                 return v4l2_subdev_call(sd, pad, enum_frame_interval, state,
 808                                         fie);
 809         }
 810
 811         case VIDIOC_SUBDEV_G_SELECTION: {
 812                 struct v4l2_subdev_selection *sel = arg;
 813
 814                 if (!client_supports_streams)
 815                         sel->stream = 0;
 816
 817                 memset(sel->reserved, 0, sizeof(sel->reserved));
 818                 return v4l2_subdev_call(
 819                         sd, pad, get_selection, state, sel);
 820         }
 821
 822         case VIDIOC_SUBDEV_S_SELECTION: {
 823                 struct v4l2_subdev_selection *sel = arg;
 824
 825                 if (sel->which != V4L2_SUBDEV_FORMAT_TRY && ro_subdev)
 826                         return -EPERM;
 827
 828                 if (!client_supports_streams)
 829                         sel->stream = 0;
 830
 831                 memset(sel->reserved, 0, sizeof(sel->reserved));
 832                 return v4l2_subdev_call(
 833                         sd, pad, set_selection, state, sel);
 834         }
 835
 836         case VIDIOC_G_EDID: {
 837                 struct v4l2_subdev_edid *edid = arg;
 838
 839                 return v4l2_subdev_call(sd, pad, get_edid, edid);
 840         }
 841
 842         case VIDIOC_S_EDID: {
 843                 struct v4l2_subdev_edid *edid = arg;
 844
 845                 return v4l2_subdev_call(sd, pad, set_edid, edid);
 846         }
 847
 848         case VIDIOC_SUBDEV_DV_TIMINGS_CAP: {
 849                 struct v4l2_dv_timings_cap *cap = arg;
 850
 851                 return v4l2_subdev_call(sd, pad, dv_timings_cap, cap);
 852         }
 853
 854         case VIDIOC_SUBDEV_ENUM_DV_TIMINGS: {
 855                 struct v4l2_enum_dv_timings *dvt = arg;
 856
 857                 return v4l2_subdev_call(sd, pad, enum_dv_timings, dvt);
 858         }
 859
 860         case VIDIOC_SUBDEV_QUERY_DV_TIMINGS:
 861                 return v4l2_subdev_call(sd, video, query_dv_timings, arg);
 862
 863         case VIDIOC_SUBDEV_G_DV_TIMINGS:
 864                 return v4l2_subdev_call(sd, video, g_dv_timings, arg);
 865
 866         case VIDIOC_SUBDEV_S_DV_TIMINGS:
 867                 if (ro_subdev)
 868                         return -EPERM;
 869
 870                 return v4l2_subdev_call(sd, video, s_dv_timings, arg);
 871
 872         case VIDIOC_SUBDEV_G_STD:
 873                 return v4l2_subdev_call(sd, video, g_std, arg);
 874
 875         case VIDIOC_SUBDEV_S_STD: {
 876                 v4l2_std_id *std = arg;
 877
 878                 if (ro_subdev)
 879                         return -EPERM;
 880
 881                 return v4l2_subdev_call(sd, video, s_std, *std);
 882         }
 883
 884         case VIDIOC_SUBDEV_ENUMSTD: {
 885                 struct v4l2_standard *p = arg;
 886                 v4l2_std_id id;
 887
 888                 if (v4l2_subdev_call(sd, video, g_tvnorms, &id))
 889                         return -EINVAL;
 890
 891                 return v4l_video_std_enumstd(p, id);
 892         }
 893
 894         case VIDIOC_SUBDEV_QUERYSTD:
 895                 return v4l2_subdev_call(sd, video, querystd, arg);
 896
 897         case VIDIOC_SUBDEV_G_ROUTING: {
 898                 struct v4l2_subdev_routing *routing = arg;
 899                 struct v4l2_subdev_krouting *krouting;
 900
 901                 if (!v4l2_subdev_enable_streams_api)
 902                         return -ENOIOCTLCMD;
 903
 904                 if (!(sd->flags & V4L2_SUBDEV_FL_STREAMS))
 905                         return -ENOIOCTLCMD;
 906
 907                 memset(routing->reserved, 0, sizeof(routing->reserved));
 908
 909                 krouting = &state->routing;
 910
 911                 if (routing->num_routes < krouting->num_routes) {
 912                         routing->num_routes = krouting->num_routes;
 913                         return -ENOSPC;
 914                 }
 915
 916                 memcpy((struct v4l2_subdev_route *)(uintptr_t)routing->routes,
 917                        krouting->routes,
 918                        krouting->num_routes * sizeof(*krouting->routes));
 919                 routing->num_routes = krouting->num_routes;
 920
 921                 return 0;
 922         }
 923
 924         case VIDIOC_SUBDEV_S_ROUTING: {
 925                 struct v4l2_subdev_routing *routing = arg;
 926                 struct v4l2_subdev_route *routes =
 927                         (struct v4l2_subdev_route *)(uintptr_t)routing->routes;
 928                 struct v4l2_subdev_krouting krouting = {};
 929                 unsigned int i;
 930
 931                 if (!v4l2_subdev_enable_streams_api)
 932                         return -ENOIOCTLCMD;
 933
 934                 if (!(sd->flags & V4L2_SUBDEV_FL_STREAMS))
 935                         return -ENOIOCTLCMD;
 936
 937                 if (routing->which != V4L2_SUBDEV_FORMAT_TRY && ro_subdev)
 938                         return -EPERM;
 939
 940                 memset(routing->reserved, 0, sizeof(routing->reserved));
 941
 942                 for (i = 0; i < routing->num_routes; ++i) {
 943                         const struct v4l2_subdev_route *route = &routes[i];
 944                         const struct media_pad *pads = sd->entity.pads;
 945
 946                         if (route->sink_stream > V4L2_SUBDEV_MAX_STREAM_ID ||
 947                             route->source_stream > V4L2_SUBDEV_MAX_STREAM_ID)
 948                                 return -EINVAL;
 949
 950                         if (route->sink_pad >= sd->entity.num_pads)
 951                                 return -EINVAL;
 952
 953                         if (!(pads[route->sink_pad].flags &
 954                               MEDIA_PAD_FL_SINK))
 955                                 return -EINVAL;
 956
 957                         if (route->source_pad >= sd->entity.num_pads)
 958                                 return -EINVAL;
 959
 960                         if (!(pads[route->source_pad].flags &
 961                               MEDIA_PAD_FL_SOURCE))
 962                                 return -EINVAL;
 963                 }
 964
 965                 krouting.num_routes = routing->num_routes;
 966                 krouting.routes = routes;
 967
 968                 return v4l2_subdev_call(sd, pad, set_routing, state,
 969                                         routing->which, &krouting);
 970         }
 971
 972         case VIDIOC_SUBDEV_G_CLIENT_CAP: {
 973                 struct v4l2_subdev_client_capability *client_cap = arg;
 974
 975                 client_cap->capabilities = subdev_fh->client_caps;
 976
 977                 return 0;
 978         }
 979
 980         case VIDIOC_SUBDEV_S_CLIENT_CAP: {
 981                 struct v4l2_subdev_client_capability *client_cap = arg;
 982
 983                 /*
 984                  * Clear V4L2_SUBDEV_CLIENT_CAP_STREAMS if streams API is not
 985                  * enabled. Remove this when streams API is no longer
 986                  * experimental.
 987                  */
 988                 if (!v4l2_subdev_enable_streams_api)
 989                         client_cap->capabilities &= ~V4L2_SUBDEV_CLIENT_CAP_STREAMS;
 990
 991                 /* Filter out unsupported capabilities */
 992                 client_cap->capabilities &= V4L2_SUBDEV_CLIENT_CAP_STREAMS;
 993
 994                 subdev_fh->client_caps = client_cap->capabilities;
 995
 996                 return 0;
 997         }
 998
 999         default:
1000                 return v4l2_subdev_call(sd, core, ioctl, cmd, arg);
1001         }
1002
1003         return 0;
1004 }
1005
1006 static long subdev_do_ioctl_lock(struct file *file, unsigned int cmd, void *arg)
1007 {
1008         struct video_device *vdev = video_devdata(file);
1009         struct mutex *lock = vdev->lock;
1010         long ret = -ENODEV;
1011
1012         if (lock && mutex_lock_interruptible(lock))
1013                 return -ERESTARTSYS;
1014
1015         if (video_is_registered(vdev)) {
1016                 struct v4l2_subdev *sd = vdev_to_v4l2_subdev(vdev);
1017                 struct v4l2_fh *vfh = file->private_data;
1018                 struct v4l2_subdev_fh *subdev_fh = to_v4l2_subdev_fh(vfh);
1019                 struct v4l2_subdev_state *state;
1020
1021                 state = subdev_ioctl_get_state(sd, subdev_fh, cmd, arg);
1022
1023                 if (state)
1024                         v4l2_subdev_lock_state(state);
1025
1026                 ret = subdev_do_ioctl(file, cmd, arg, state);
1027
1028                 if (state)
1029                         v4l2_subdev_unlock_state(state);
1030         }
1031
1032         if (lock)
1033                 mutex_unlock(lock);
1034         return ret;
1035 }
1036
1037 static long subdev_ioctl(struct file *file, unsigned int cmd,
1038         unsigned long arg)
1039 {
1040         return video_usercopy(file, cmd, arg, subdev_do_ioctl_lock);
1041 }
1042
1043 #ifdef CONFIG_COMPAT
1044 static long subdev_compat_ioctl32(struct file *file, unsigned int cmd,
1045         unsigned long arg)
1046 {
1047         struct video_device *vdev = video_devdata(file);
1048         struct v4l2_subdev *sd = vdev_to_v4l2_subdev(vdev);
1049
1050         return v4l2_subdev_call(sd, core, compat_ioctl32, cmd, arg);
1051 }
1052 #endif
1053
1054 #else /* CONFIG_VIDEO_V4L2_SUBDEV_API */
1055 static long subdev_ioctl(struct file *file, unsigned int cmd,
1056                          unsigned long arg)
1057 {
1058         return -ENODEV;
1059 }
1060
1061 #ifdef CONFIG_COMPAT
1062 static long subdev_compat_ioctl32(struct file *file, unsigned int cmd,
1063                                   unsigned long arg)
1064 {
1065         return -ENODEV;
1066 }
1067 #endif
1068 #endif /* CONFIG_VIDEO_V4L2_SUBDEV_API */
1069
1070 static __poll_t subdev_poll(struct file *file, poll_table *wait)
1071 {
1072         struct video_device *vdev = video_devdata(file);
1073         struct v4l2_subdev *sd = vdev_to_v4l2_subdev(vdev);
1074         struct v4l2_fh *fh = file->private_data;
1075
1076         if (!(sd->flags & V4L2_SUBDEV_FL_HAS_EVENTS))
1077                 return EPOLLERR;
1078
1079         poll_wait(file, &fh->wait, wait);
1080
1081         if (v4l2_event_pending(fh))
1082                 return EPOLLPRI;
1083
1084         return 0;
1085 }
1086
1087 const struct v4l2_file_operations v4l2_subdev_fops = {
1088         .owner = THIS_MODULE,
1089         .open = subdev_open,
1090         .unlocked_ioctl = subdev_ioctl,
1091 #ifdef CONFIG_COMPAT
1092         .compat_ioctl32 = subdev_compat_ioctl32,
1093 #endif
1094         .release = subdev_close,
1095         .poll = subdev_poll,
1096 };
1097
1098 #ifdef CONFIG_MEDIA_CONTROLLER
1099
1100 int v4l2_subdev_get_fwnode_pad_1_to_1(struct media_entity *entity,
1101                                       struct fwnode_endpoint *endpoint)
1102 {
1103         struct fwnode_handle *fwnode;
1104         struct v4l2_subdev *sd;
1105
1106         if (!is_media_entity_v4l2_subdev(entity))
1107                 return -EINVAL;
1108
1109         sd = media_entity_to_v4l2_subdev(entity);
1110
1111         fwnode = fwnode_graph_get_port_parent(endpoint->local_fwnode);
1112         fwnode_handle_put(fwnode);
1113
1114         if (device_match_fwnode(sd->dev, fwnode))
1115                 return endpoint->port;
1116
1117         return -ENXIO;
1118 }
1119 EXPORT_SYMBOL_GPL(v4l2_subdev_get_fwnode_pad_1_to_1);
1120
1121 int v4l2_subdev_link_validate_default(struct v4l2_subdev *sd,
1122                                       struct media_link *link,
1123                                       struct v4l2_subdev_format *source_fmt,
1124                                       struct v4l2_subdev_format *sink_fmt)
1125 {
1126         bool pass = true;
1127
1128         /* The width, height and code must match. */
1129         if (source_fmt->format.width != sink_fmt->format.width) {
1130                 dev_dbg(sd->entity.graph_obj.mdev->dev,
1131                         "%s: width does not match (source %u, sink %u)\n",
1132                         __func__,
1133                         source_fmt->format.width, sink_fmt->format.width);
1134                 pass = false;
1135         }
1136
1137         if (source_fmt->format.height != sink_fmt->format.height) {
1138                 dev_dbg(sd->entity.graph_obj.mdev->dev,
1139                         "%s: height does not match (source %u, sink %u)\n",
1140                         __func__,
1141                         source_fmt->format.height, sink_fmt->format.height);
1142                 pass = false;
1143         }
1144
1145         if (source_fmt->format.code != sink_fmt->format.code) {
1146                 dev_dbg(sd->entity.graph_obj.mdev->dev,
1147                         "%s: media bus code does not match (source 0x%8.8x, sink 0x%8.8x)\n",
1148                         __func__,
1149                         source_fmt->format.code, sink_fmt->format.code);
1150                 pass = false;
1151         }
1152
1153         /* The field order must match, or the sink field order must be NONE
1154          * to support interlaced hardware connected to bridges that support
1155          * progressive formats only.
1156          */
1157         if (source_fmt->format.field != sink_fmt->format.field &&
1158             sink_fmt->format.field != V4L2_FIELD_NONE) {
1159                 dev_dbg(sd->entity.graph_obj.mdev->dev,
1160                         "%s: field does not match (source %u, sink %u)\n",
1161                         __func__,
1162                         source_fmt->format.field, sink_fmt->format.field);
1163                 pass = false;
1164         }
1165
1166         if (pass)
1167                 return 0;
1168
1169         dev_dbg(sd->entity.graph_obj.mdev->dev,
1170                 "%s: link was \"%s\":%u -> \"%s\":%u\n", __func__,
1171                 link->source->entity->name, link->source->index,
1172                 link->sink->entity->name, link->sink->index);
1173
1174         return -EPIPE;
1175 }
1176 EXPORT_SYMBOL_GPL(v4l2_subdev_link_validate_default);
1177
1178 static int
1179 v4l2_subdev_link_validate_get_format(struct media_pad *pad, u32 stream,
1180                                      struct v4l2_subdev_format *fmt,
1181                                      bool states_locked)
1182 {
1183         struct v4l2_subdev_state *state;
1184         struct v4l2_subdev *sd;
1185         int ret;
1186
1187         if (!is_media_entity_v4l2_subdev(pad->entity)) {
1188                 WARN(pad->entity->function != MEDIA_ENT_F_IO_V4L,
1189                      "Driver bug! Wrong media entity type 0x%08x, entity %s\n",
1190                      pad->entity->function, pad->entity->name);
1191
1192                 return -EINVAL;
1193         }
1194
1195         sd = media_entity_to_v4l2_subdev(pad->entity);
1196
1197         fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
1198         fmt->pad = pad->index;
1199         fmt->stream = stream;
1200
1201         if (states_locked)
1202                 state = v4l2_subdev_get_locked_active_state(sd);
1203         else
1204                 state = v4l2_subdev_lock_and_get_active_state(sd);
1205
1206         ret = v4l2_subdev_call(sd, pad, get_fmt, state, fmt);
1207
1208         if (!states_locked && state)
1209                 v4l2_subdev_unlock_state(state);
1210
1211         return ret;
1212 }
1213
1214 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
1215
1216 static void __v4l2_link_validate_get_streams(struct media_pad *pad,
1217                                              u64 *streams_mask,
1218                                              bool states_locked)
1219 {
1220         struct v4l2_subdev_route *route;
1221         struct v4l2_subdev_state *state;
1222         struct v4l2_subdev *subdev;
1223
1224         subdev = media_entity_to_v4l2_subdev(pad->entity);
1225
1226         *streams_mask = 0;
1227
1228         if (states_locked)
1229                 state = v4l2_subdev_get_locked_active_state(subdev);
1230         else
1231                 state = v4l2_subdev_lock_and_get_active_state(subdev);
1232
1233         if (WARN_ON(!state))
1234                 return;
1235
1236         for_each_active_route(&state->routing, route) {
1237                 u32 route_pad;
1238                 u32 route_stream;
1239
1240                 if (pad->flags & MEDIA_PAD_FL_SOURCE) {
1241                         route_pad = route->source_pad;
1242                         route_stream = route->source_stream;
1243                 } else {
1244                         route_pad = route->sink_pad;
1245                         route_stream = route->sink_stream;
1246                 }
1247
1248                 if (route_pad != pad->index)
1249                         continue;
1250
1251                 *streams_mask |= BIT_ULL(route_stream);
1252         }
1253
1254         if (!states_locked)
1255                 v4l2_subdev_unlock_state(state);
1256 }
1257
1258 #endif /* CONFIG_VIDEO_V4L2_SUBDEV_API */
1259
1260 static void v4l2_link_validate_get_streams(struct media_pad *pad,
1261                                            u64 *streams_mask,
1262                                            bool states_locked)
1263 {
1264         struct v4l2_subdev *subdev = media_entity_to_v4l2_subdev(pad->entity);
1265
1266         if (!(subdev->flags & V4L2_SUBDEV_FL_STREAMS)) {
1267                 /* Non-streams subdevs have an implicit stream 0 */
1268                 *streams_mask = BIT_ULL(0);
1269                 return;
1270         }
1271
1272 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
1273         __v4l2_link_validate_get_streams(pad, streams_mask, states_locked);
1274 #else
1275         /* This shouldn't happen */
1276         *streams_mask = 0;
1277 #endif
1278 }
1279
1280 static int v4l2_subdev_link_validate_locked(struct media_link *link, bool states_locked)
1281 {
1282         struct v4l2_subdev *sink_subdev =
1283                 media_entity_to_v4l2_subdev(link->sink->entity);
1284         struct device *dev = sink_subdev->entity.graph_obj.mdev->dev;
1285         u64 source_streams_mask;
1286         u64 sink_streams_mask;
1287         u64 dangling_sink_streams;
1288         u32 stream;
1289         int ret;
1290
1291         dev_dbg(dev, "validating link \"%s\":%u -> \"%s\":%u\n",
1292                 link->source->entity->name, link->source->index,
1293                 link->sink->entity->name, link->sink->index);
1294
1295         v4l2_link_validate_get_streams(link->source, &source_streams_mask, states_locked);
1296         v4l2_link_validate_get_streams(link->sink, &sink_streams_mask, states_locked);
1297
1298         /*
1299          * It is ok to have more source streams than sink streams as extra
1300          * source streams can just be ignored by the receiver, but having extra
1301          * sink streams is an error as streams must have a source.
1302          */
1303         dangling_sink_streams = (source_streams_mask ^ sink_streams_mask) &
1304                                 sink_streams_mask;
1305         if (dangling_sink_streams) {
1306                 dev_err(dev, "Dangling sink streams: mask %#llx\n",
1307                         dangling_sink_streams);
1308                 return -EINVAL;
1309         }
1310
1311         /* Validate source and sink stream formats */
1312
1313         for (stream = 0; stream < sizeof(sink_streams_mask) * 8; ++stream) {
1314                 struct v4l2_subdev_format sink_fmt, source_fmt;
1315
1316                 if (!(sink_streams_mask & BIT_ULL(stream)))
1317                         continue;
1318
1319                 dev_dbg(dev, "validating stream \"%s\":%u:%u -> \"%s\":%u:%u\n",
1320                         link->source->entity->name, link->source->index, stream,
1321                         link->sink->entity->name, link->sink->index, stream);
1322
1323                 ret = v4l2_subdev_link_validate_get_format(link->source, stream,
1324                                                            &source_fmt, states_locked);
1325                 if (ret < 0) {
1326                         dev_dbg(dev,
1327                                 "Failed to get format for \"%s\":%u:%u (but that's ok)\n",
1328                                 link->source->entity->name, link->source->index,
1329                                 stream);
1330                         continue;
1331                 }
1332
1333                 ret = v4l2_subdev_link_validate_get_format(link->sink, stream,
1334                                                            &sink_fmt, states_locked);
1335                 if (ret < 0) {
1336                         dev_dbg(dev,
1337                                 "Failed to get format for \"%s\":%u:%u (but that's ok)\n",
1338                                 link->sink->entity->name, link->sink->index,
1339                                 stream);
1340                         continue;
1341                 }
1342
1343                 /* TODO: add stream number to link_validate() */
1344                 ret = v4l2_subdev_call(sink_subdev, pad, link_validate, link,
1345                                        &source_fmt, &sink_fmt);
1346                 if (!ret)
1347                         continue;
1348
1349                 if (ret != -ENOIOCTLCMD)
1350                         return ret;
1351
1352                 ret = v4l2_subdev_link_validate_default(sink_subdev, link,
1353                                                         &source_fmt, &sink_fmt);
1354
1355                 if (ret)
1356                         return ret;
1357         }
1358
1359         return 0;
1360 }
1361
1362 int v4l2_subdev_link_validate(struct media_link *link)
1363 {
1364         struct v4l2_subdev *source_sd, *sink_sd;
1365         struct v4l2_subdev_state *source_state, *sink_state;
1366         bool states_locked;
1367         int ret;
1368
1369         if (!is_media_entity_v4l2_subdev(link->sink->entity) ||
1370             !is_media_entity_v4l2_subdev(link->source->entity)) {
1371                 pr_warn_once("%s of link '%s':%u->'%s':%u is not a V4L2 sub-device, driver bug!\n",
1372                              !is_media_entity_v4l2_subdev(link->sink->entity) ?
1373                              "sink" : "source",
1374                              link->source->entity->name, link->source->index,
1375                              link->sink->entity->name, link->sink->index);
1376                 return 0;
1377         }
1378
1379         sink_sd = media_entity_to_v4l2_subdev(link->sink->entity);
1380         source_sd = media_entity_to_v4l2_subdev(link->source->entity);
1381
1382         sink_state = v4l2_subdev_get_unlocked_active_state(sink_sd);
1383         source_state = v4l2_subdev_get_unlocked_active_state(source_sd);
1384
1385         states_locked = sink_state && source_state;
1386
1387         if (states_locked) {
1388                 v4l2_subdev_lock_state(sink_state);
1389                 v4l2_subdev_lock_state(source_state);
1390         }
1391
1392         ret = v4l2_subdev_link_validate_locked(link, states_locked);
1393
1394         if (states_locked) {
1395                 v4l2_subdev_unlock_state(sink_state);
1396                 v4l2_subdev_unlock_state(source_state);
1397         }
1398
1399         return ret;
1400 }
1401 EXPORT_SYMBOL_GPL(v4l2_subdev_link_validate);
1402
1403 bool v4l2_subdev_has_pad_interdep(struct media_entity *entity,
1404                                   unsigned int pad0, unsigned int pad1)
1405 {
1406         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
1407         struct v4l2_subdev_krouting *routing;
1408         struct v4l2_subdev_state *state;
1409         unsigned int i;
1410
1411         state = v4l2_subdev_lock_and_get_active_state(sd);
1412
1413         routing = &state->routing;
1414
1415         for (i = 0; i < routing->num_routes; ++i) {
1416                 struct v4l2_subdev_route *route = &routing->routes[i];
1417
1418                 if (!(route->flags & V4L2_SUBDEV_ROUTE_FL_ACTIVE))
1419                         continue;
1420
1421                 if ((route->sink_pad == pad0 && route->source_pad == pad1) ||
1422                     (route->source_pad == pad0 && route->sink_pad == pad1)) {
1423                         v4l2_subdev_unlock_state(state);
1424                         return true;
1425                 }
1426         }
1427
1428         v4l2_subdev_unlock_state(state);
1429
1430         return false;
1431 }
1432 EXPORT_SYMBOL_GPL(v4l2_subdev_has_pad_interdep);
1433
1434 struct v4l2_subdev_state *
1435 __v4l2_subdev_state_alloc(struct v4l2_subdev *sd, const char *lock_name,
1436                           struct lock_class_key *lock_key)
1437 {
1438         struct v4l2_subdev_state *state;
1439         int ret;
1440
1441         state = kzalloc(sizeof(*state), GFP_KERNEL);
1442         if (!state)
1443                 return ERR_PTR(-ENOMEM);
1444
1445         __mutex_init(&state->_lock, lock_name, lock_key);
1446         if (sd->state_lock)
1447                 state->lock = sd->state_lock;
1448         else
1449                 state->lock = &state->_lock;
1450
1451         /* Drivers that support streams do not need the legacy pad config */
1452         if (!(sd->flags & V4L2_SUBDEV_FL_STREAMS) && sd->entity.num_pads) {
1453                 state->pads = kvcalloc(sd->entity.num_pads,
1454                                        sizeof(*state->pads), GFP_KERNEL);
1455                 if (!state->pads) {
1456                         ret = -ENOMEM;
1457                         goto err;
1458                 }
1459         }
1460
1461         /*
1462          * There can be no race at this point, but we lock the state anyway to
1463          * satisfy lockdep checks.
1464          */
1465         v4l2_subdev_lock_state(state);
1466         ret = v4l2_subdev_call(sd, pad, init_cfg, state);
1467         v4l2_subdev_unlock_state(state);
1468
1469         if (ret < 0 && ret != -ENOIOCTLCMD)
1470                 goto err;
1471
1472         return state;
1473
1474 err:
1475         if (state && state->pads)
1476                 kvfree(state->pads);
1477
1478         kfree(state);
1479
1480         return ERR_PTR(ret);
1481 }
1482 EXPORT_SYMBOL_GPL(__v4l2_subdev_state_alloc);
1483
1484 void __v4l2_subdev_state_free(struct v4l2_subdev_state *state)
1485 {
1486         if (!state)
1487                 return;
1488
1489         mutex_destroy(&state->_lock);
1490
1491         kfree(state->routing.routes);
1492         kvfree(state->stream_configs.configs);
1493         kvfree(state->pads);
1494         kfree(state);
1495 }
1496 EXPORT_SYMBOL_GPL(__v4l2_subdev_state_free);
1497
1498 int __v4l2_subdev_init_finalize(struct v4l2_subdev *sd, const char *name,
1499                                 struct lock_class_key *key)
1500 {
1501         struct v4l2_subdev_state *state;
1502
1503         state = __v4l2_subdev_state_alloc(sd, name, key);
1504         if (IS_ERR(state))
1505                 return PTR_ERR(state);
1506
1507         sd->active_state = state;
1508
1509         return 0;
1510 }
1511 EXPORT_SYMBOL_GPL(__v4l2_subdev_init_finalize);
1512
1513 void v4l2_subdev_cleanup(struct v4l2_subdev *sd)
1514 {
1515         struct v4l2_async_subdev_endpoint *ase, *ase_tmp;
1516
1517         __v4l2_subdev_state_free(sd->active_state);
1518         sd->active_state = NULL;
1519
1520         if (list_empty(&sd->async_subdev_endpoint_list))
1521                 return;
1522
1523         list_for_each_entry_safe(ase, ase_tmp, &sd->async_subdev_endpoint_list,
1524                                  async_subdev_endpoint_entry) {
1525                 list_del(&ase->async_subdev_endpoint_entry);
1526
1527                 kfree(ase);
1528         }
1529 }
1530 EXPORT_SYMBOL_GPL(v4l2_subdev_cleanup);
1531
1532 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
1533
1534 static int
1535 v4l2_subdev_init_stream_configs(struct v4l2_subdev_stream_configs *stream_configs,
1536                                 const struct v4l2_subdev_krouting *routing)
1537 {
1538         struct v4l2_subdev_stream_configs new_configs = { 0 };
1539         struct v4l2_subdev_route *route;
1540         u32 idx;
1541
1542         /* Count number of formats needed */
1543         for_each_active_route(routing, route) {
1544                 /*
1545                  * Each route needs a format on both ends of the route.
1546                  */
1547                 new_configs.num_configs += 2;
1548         }
1549
1550         if (new_configs.num_configs) {
1551                 new_configs.configs = kvcalloc(new_configs.num_configs,
1552                                                sizeof(*new_configs.configs),
1553                                                GFP_KERNEL);
1554
1555                 if (!new_configs.configs)
1556                         return -ENOMEM;
1557         }
1558
1559         /*
1560          * Fill in the 'pad' and stream' value for each item in the array from
1561          * the routing table
1562          */
1563         idx = 0;
1564
1565         for_each_active_route(routing, route) {
1566                 new_configs.configs[idx].pad = route->sink_pad;
1567                 new_configs.configs[idx].stream = route->sink_stream;
1568
1569                 idx++;
1570
1571                 new_configs.configs[idx].pad = route->source_pad;
1572                 new_configs.configs[idx].stream = route->source_stream;
1573
1574                 idx++;
1575         }
1576
1577         kvfree(stream_configs->configs);
1578         *stream_configs = new_configs;
1579
1580         return 0;
1581 }
1582
1583 int v4l2_subdev_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_state *state,
1584                         struct v4l2_subdev_format *format)
1585 {
1586         struct v4l2_mbus_framefmt *fmt;
1587
1588         if (sd->flags & V4L2_SUBDEV_FL_STREAMS)
1589                 fmt = v4l2_subdev_state_get_stream_format(state, format->pad,
1590                                                           format->stream);
1591         else if (format->pad < sd->entity.num_pads && format->stream == 0)
1592                 fmt = v4l2_subdev_get_pad_format(sd, state, format->pad);
1593         else
1594                 fmt = NULL;
1595
1596         if (!fmt)
1597                 return -EINVAL;
1598
1599         format->format = *fmt;
1600
1601         return 0;
1602 }
1603 EXPORT_SYMBOL_GPL(v4l2_subdev_get_fmt);
1604
1605 int v4l2_subdev_set_routing(struct v4l2_subdev *sd,
1606                             struct v4l2_subdev_state *state,
1607                             const struct v4l2_subdev_krouting *routing)
1608 {
1609         struct v4l2_subdev_krouting *dst = &state->routing;
1610         const struct v4l2_subdev_krouting *src = routing;
1611         struct v4l2_subdev_krouting new_routing = { 0 };
1612         size_t bytes;
1613         int r;
1614
1615         if (unlikely(check_mul_overflow((size_t)src->num_routes,
1616                                         sizeof(*src->routes), &bytes)))
1617                 return -EOVERFLOW;
1618
1619         lockdep_assert_held(state->lock);
1620
1621         if (src->num_routes > 0) {
1622                 new_routing.routes = kmemdup(src->routes, bytes, GFP_KERNEL);
1623                 if (!new_routing.routes)
1624                         return -ENOMEM;
1625         }
1626
1627         new_routing.num_routes = src->num_routes;
1628
1629         r = v4l2_subdev_init_stream_configs(&state->stream_configs,
1630                                             &new_routing);
1631         if (r) {
1632                 kfree(new_routing.routes);
1633                 return r;
1634         }
1635
1636         kfree(dst->routes);
1637         *dst = new_routing;
1638
1639         return 0;
1640 }
1641 EXPORT_SYMBOL_GPL(v4l2_subdev_set_routing);
1642
1643 struct v4l2_subdev_route *
1644 __v4l2_subdev_next_active_route(const struct v4l2_subdev_krouting *routing,
1645                                 struct v4l2_subdev_route *route)
1646 {
1647         if (route)
1648                 ++route;
1649         else
1650                 route = &routing->routes[0];
1651
1652         for (; route < routing->routes + routing->num_routes; ++route) {
1653                 if (!(route->flags & V4L2_SUBDEV_ROUTE_FL_ACTIVE))
1654                         continue;
1655
1656                 return route;
1657         }
1658
1659         return NULL;
1660 }
1661 EXPORT_SYMBOL_GPL(__v4l2_subdev_next_active_route);
1662
1663 int v4l2_subdev_set_routing_with_fmt(struct v4l2_subdev *sd,
1664                                      struct v4l2_subdev_state *state,
1665                                      const struct v4l2_subdev_krouting *routing,
1666                                      const struct v4l2_mbus_framefmt *fmt)
1667 {
1668         struct v4l2_subdev_stream_configs *stream_configs;
1669         unsigned int i;
1670         int ret;
1671
1672         ret = v4l2_subdev_set_routing(sd, state, routing);
1673         if (ret)
1674                 return ret;
1675
1676         stream_configs = &state->stream_configs;
1677
1678         for (i = 0; i < stream_configs->num_configs; ++i)
1679                 stream_configs->configs[i].fmt = *fmt;
1680
1681         return 0;
1682 }
1683 EXPORT_SYMBOL_GPL(v4l2_subdev_set_routing_with_fmt);
1684
1685 struct v4l2_mbus_framefmt *
1686 v4l2_subdev_state_get_stream_format(struct v4l2_subdev_state *state,
1687                                     unsigned int pad, u32 stream)
1688 {
1689         struct v4l2_subdev_stream_configs *stream_configs;
1690         unsigned int i;
1691
1692         lockdep_assert_held(state->lock);
1693
1694         stream_configs = &state->stream_configs;
1695
1696         for (i = 0; i < stream_configs->num_configs; ++i) {
1697                 if (stream_configs->configs[i].pad == pad &&
1698                     stream_configs->configs[i].stream == stream)
1699                         return &stream_configs->configs[i].fmt;
1700         }
1701
1702         return NULL;
1703 }
1704 EXPORT_SYMBOL_GPL(v4l2_subdev_state_get_stream_format);
1705
1706 struct v4l2_rect *
1707 v4l2_subdev_state_get_stream_crop(struct v4l2_subdev_state *state,
1708                                   unsigned int pad, u32 stream)
1709 {
1710         struct v4l2_subdev_stream_configs *stream_configs;
1711         unsigned int i;
1712
1713         lockdep_assert_held(state->lock);
1714
1715         stream_configs = &state->stream_configs;
1716
1717         for (i = 0; i < stream_configs->num_configs; ++i) {
1718                 if (stream_configs->configs[i].pad == pad &&
1719                     stream_configs->configs[i].stream == stream)
1720                         return &stream_configs->configs[i].crop;
1721         }
1722
1723         return NULL;
1724 }
1725 EXPORT_SYMBOL_GPL(v4l2_subdev_state_get_stream_crop);
1726
1727 struct v4l2_rect *
1728 v4l2_subdev_state_get_stream_compose(struct v4l2_subdev_state *state,
1729                                      unsigned int pad, u32 stream)
1730 {
1731         struct v4l2_subdev_stream_configs *stream_configs;
1732         unsigned int i;
1733
1734         lockdep_assert_held(state->lock);
1735
1736         stream_configs = &state->stream_configs;
1737
1738         for (i = 0; i < stream_configs->num_configs; ++i) {
1739                 if (stream_configs->configs[i].pad == pad &&
1740                     stream_configs->configs[i].stream == stream)
1741                         return &stream_configs->configs[i].compose;
1742         }
1743
1744         return NULL;
1745 }
1746 EXPORT_SYMBOL_GPL(v4l2_subdev_state_get_stream_compose);
1747
1748 int v4l2_subdev_routing_find_opposite_end(const struct v4l2_subdev_krouting *routing,
1749                                           u32 pad, u32 stream, u32 *other_pad,
1750                                           u32 *other_stream)
1751 {
1752         unsigned int i;
1753
1754         for (i = 0; i < routing->num_routes; ++i) {
1755                 struct v4l2_subdev_route *route = &routing->routes[i];
1756
1757                 if (route->source_pad == pad &&
1758                     route->source_stream == stream) {
1759                         if (other_pad)
1760                                 *other_pad = route->sink_pad;
1761                         if (other_stream)
1762                                 *other_stream = route->sink_stream;
1763                         return 0;
1764                 }
1765
1766                 if (route->sink_pad == pad && route->sink_stream == stream) {
1767                         if (other_pad)
1768                                 *other_pad = route->source_pad;
1769                         if (other_stream)
1770                                 *other_stream = route->source_stream;
1771                         return 0;
1772                 }
1773         }
1774
1775         return -EINVAL;
1776 }
1777 EXPORT_SYMBOL_GPL(v4l2_subdev_routing_find_opposite_end);
1778
1779 struct v4l2_mbus_framefmt *
1780 v4l2_subdev_state_get_opposite_stream_format(struct v4l2_subdev_state *state,
1781                                              u32 pad, u32 stream)
1782 {
1783         u32 other_pad, other_stream;
1784         int ret;
1785
1786         ret = v4l2_subdev_routing_find_opposite_end(&state->routing,
1787                                                     pad, stream,
1788                                                     &other_pad, &other_stream);
1789         if (ret)
1790                 return NULL;
1791
1792         return v4l2_subdev_state_get_stream_format(state, other_pad,
1793                                                    other_stream);
1794 }
1795 EXPORT_SYMBOL_GPL(v4l2_subdev_state_get_opposite_stream_format);
1796
1797 u64 v4l2_subdev_state_xlate_streams(const struct v4l2_subdev_state *state,
1798                                     u32 pad0, u32 pad1, u64 *streams)
1799 {
1800         const struct v4l2_subdev_krouting *routing = &state->routing;
1801         struct v4l2_subdev_route *route;
1802         u64 streams0 = 0;
1803         u64 streams1 = 0;
1804
1805         for_each_active_route(routing, route) {
1806                 if (route->sink_pad == pad0 && route->source_pad == pad1 &&
1807                     (*streams & BIT_ULL(route->sink_stream))) {
1808                         streams0 |= BIT_ULL(route->sink_stream);
1809                         streams1 |= BIT_ULL(route->source_stream);
1810                 }
1811                 if (route->source_pad == pad0 && route->sink_pad == pad1 &&
1812                     (*streams & BIT_ULL(route->source_stream))) {
1813                         streams0 |= BIT_ULL(route->source_stream);
1814                         streams1 |= BIT_ULL(route->sink_stream);
1815                 }
1816         }
1817
1818         *streams = streams0;
1819         return streams1;
1820 }
1821 EXPORT_SYMBOL_GPL(v4l2_subdev_state_xlate_streams);
1822
1823 int v4l2_subdev_routing_validate(struct v4l2_subdev *sd,
1824                                  const struct v4l2_subdev_krouting *routing,
1825                                  enum v4l2_subdev_routing_restriction disallow)
1826 {
1827         u32 *remote_pads = NULL;
1828         unsigned int i, j;
1829         int ret = -EINVAL;
1830
1831         if (disallow & (V4L2_SUBDEV_ROUTING_NO_STREAM_MIX |
1832                         V4L2_SUBDEV_ROUTING_NO_MULTIPLEXING)) {
1833                 remote_pads = kcalloc(sd->entity.num_pads, sizeof(*remote_pads),
1834                                       GFP_KERNEL);
1835                 if (!remote_pads)
1836                         return -ENOMEM;
1837
1838                 for (i = 0; i < sd->entity.num_pads; ++i)
1839                         remote_pads[i] = U32_MAX;
1840         }
1841
1842         for (i = 0; i < routing->num_routes; ++i) {
1843                 const struct v4l2_subdev_route *route = &routing->routes[i];
1844
1845                 /* Validate the sink and source pad numbers. */
1846                 if (route->sink_pad >= sd->entity.num_pads ||
1847                     !(sd->entity.pads[route->sink_pad].flags & MEDIA_PAD_FL_SINK)) {
1848                         dev_dbg(sd->dev, "route %u sink (%u) is not a sink pad\n",
1849                                 i, route->sink_pad);
1850                         goto out;
1851                 }
1852
1853                 if (route->source_pad >= sd->entity.num_pads ||
1854                     !(sd->entity.pads[route->source_pad].flags & MEDIA_PAD_FL_SOURCE)) {
1855                         dev_dbg(sd->dev, "route %u source (%u) is not a source pad\n",
1856                                 i, route->source_pad);
1857                         goto out;
1858                 }
1859
1860                 /*
1861                  * V4L2_SUBDEV_ROUTING_NO_SINK_STREAM_MIX: all streams from a
1862                  * sink pad must be routed to a single source pad.
1863                  */
1864                 if (disallow & V4L2_SUBDEV_ROUTING_NO_SINK_STREAM_MIX) {
1865                         if (remote_pads[route->sink_pad] != U32_MAX &&
1866                             remote_pads[route->sink_pad] != route->source_pad) {
1867                                 dev_dbg(sd->dev,
1868                                         "route %u attempts to mix %s streams\n",
1869                                         i, "sink");
1870                                 goto out;
1871                         }
1872                 }
1873
1874                 /*
1875                  * V4L2_SUBDEV_ROUTING_NO_SOURCE_STREAM_MIX: all streams on a
1876                  * source pad must originate from a single sink pad.
1877                  */
1878                 if (disallow & V4L2_SUBDEV_ROUTING_NO_SOURCE_STREAM_MIX) {
1879                         if (remote_pads[route->source_pad] != U32_MAX &&
1880                             remote_pads[route->source_pad] != route->sink_pad) {
1881                                 dev_dbg(sd->dev,
1882                                         "route %u attempts to mix %s streams\n",
1883                                         i, "source");
1884                                 goto out;
1885                         }
1886                 }
1887
1888                 /*
1889                  * V4L2_SUBDEV_ROUTING_NO_SINK_MULTIPLEXING: Pads on the sink
1890                  * side can not do stream multiplexing, i.e. there can be only
1891                  * a single stream in a sink pad.
1892                  */
1893                 if (disallow & V4L2_SUBDEV_ROUTING_NO_SINK_MULTIPLEXING) {
1894                         if (remote_pads[route->sink_pad] != U32_MAX) {
1895                                 dev_dbg(sd->dev,
1896                                         "route %u attempts to multiplex on %s pad %u\n",
1897                                         i, "sink", route->sink_pad);
1898                                 goto out;
1899                         }
1900                 }
1901
1902                 /*
1903                  * V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING: Pads on the
1904                  * source side can not do stream multiplexing, i.e. there can
1905                  * be only a single stream in a source pad.
1906                  */
1907                 if (disallow & V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING) {
1908                         if (remote_pads[route->source_pad] != U32_MAX) {
1909                                 dev_dbg(sd->dev,
1910                                         "route %u attempts to multiplex on %s pad %u\n",
1911                                         i, "source", route->source_pad);
1912                                 goto out;
1913                         }
1914                 }
1915
1916                 if (remote_pads) {
1917                         remote_pads[route->sink_pad] = route->source_pad;
1918                         remote_pads[route->source_pad] = route->sink_pad;
1919                 }
1920
1921                 for (j = i + 1; j < routing->num_routes; ++j) {
1922                         const struct v4l2_subdev_route *r = &routing->routes[j];
1923
1924                         /*
1925                          * V4L2_SUBDEV_ROUTING_NO_1_TO_N: No two routes can
1926                          * originate from the same (sink) stream.
1927                          */
1928                         if ((disallow & V4L2_SUBDEV_ROUTING_NO_1_TO_N) &&
1929                             route->sink_pad == r->sink_pad &&
1930                             route->sink_stream == r->sink_stream) {
1931                                 dev_dbg(sd->dev,
1932                                         "routes %u and %u originate from same sink (%u/%u)\n",
1933                                         i, j, route->sink_pad,
1934                                         route->sink_stream);
1935                                 goto out;
1936                         }
1937
1938                         /*
1939                          * V4L2_SUBDEV_ROUTING_NO_N_TO_1: No two routes can end
1940                          * at the same (source) stream.
1941                          */
1942                         if ((disallow & V4L2_SUBDEV_ROUTING_NO_N_TO_1) &&
1943                             route->source_pad == r->source_pad &&
1944                             route->source_stream == r->source_stream) {
1945                                 dev_dbg(sd->dev,
1946                                         "routes %u and %u end at same source (%u/%u)\n",
1947                                         i, j, route->source_pad,
1948                                         route->source_stream);
1949                                 goto out;
1950                         }
1951                 }
1952         }
1953
1954         ret = 0;
1955
1956 out:
1957         kfree(remote_pads);
1958         return ret;
1959 }
1960 EXPORT_SYMBOL_GPL(v4l2_subdev_routing_validate);
1961
1962 static int v4l2_subdev_enable_streams_fallback(struct v4l2_subdev *sd, u32 pad,
1963                                                u64 streams_mask)
1964 {
1965         struct device *dev = sd->entity.graph_obj.mdev->dev;
1966         unsigned int i;
1967         int ret;
1968
1969         /*
1970          * The subdev doesn't implement pad-based stream enable, fall back
1971          * on the .s_stream() operation. This can only be done for subdevs that
1972          * have a single source pad, as sd->enabled_streams is global to the
1973          * subdev.
1974          */
1975         if (!(sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE))
1976                 return -EOPNOTSUPP;
1977
1978         for (i = 0; i < sd->entity.num_pads; ++i) {
1979                 if (i != pad && sd->entity.pads[i].flags & MEDIA_PAD_FL_SOURCE)
1980                         return -EOPNOTSUPP;
1981         }
1982
1983         if (sd->enabled_streams & streams_mask) {
1984                 dev_dbg(dev, "set of streams %#llx already enabled on %s:%u\n",
1985                         streams_mask, sd->entity.name, pad);
1986                 return -EALREADY;
1987         }
1988
1989         /* Start streaming when the first streams are enabled. */
1990         if (!sd->enabled_streams) {
1991                 ret = v4l2_subdev_call(sd, video, s_stream, 1);
1992                 if (ret)
1993                         return ret;
1994         }
1995
1996         sd->enabled_streams |= streams_mask;
1997
1998         return 0;
1999 }
2000
2001 int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
2002                                u64 streams_mask)
2003 {
2004         struct device *dev = sd->entity.graph_obj.mdev->dev;
2005         struct v4l2_subdev_state *state;
2006         u64 found_streams = 0;
2007         unsigned int i;
2008         int ret;
2009
2010         /* A few basic sanity checks first. */
2011         if (pad >= sd->entity.num_pads)
2012                 return -EINVAL;
2013
2014         if (!streams_mask)
2015                 return 0;
2016
2017         /* Fallback on .s_stream() if .enable_streams() isn't available. */
2018         if (!sd->ops->pad || !sd->ops->pad->enable_streams)
2019                 return v4l2_subdev_enable_streams_fallback(sd, pad,
2020                                                            streams_mask);
2021
2022         state = v4l2_subdev_lock_and_get_active_state(sd);
2023
2024         /*
2025          * Verify that the requested streams exist and that they are not
2026          * already enabled.
2027          */
2028         for (i = 0; i < state->stream_configs.num_configs; ++i) {
2029                 struct v4l2_subdev_stream_config *cfg =
2030                         &state->stream_configs.configs[i];
2031
2032                 if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
2033                         continue;
2034
2035                 found_streams |= BIT_ULL(cfg->stream);
2036
2037                 if (cfg->enabled) {
2038                         dev_dbg(dev, "stream %u already enabled on %s:%u\n",
2039                                 cfg->stream, sd->entity.name, pad);
2040                         ret = -EALREADY;
2041                         goto done;
2042                 }
2043         }
2044
2045         if (found_streams != streams_mask) {
2046                 dev_dbg(dev, "streams 0x%llx not found on %s:%u\n",
2047                         streams_mask & ~found_streams, sd->entity.name, pad);
2048                 ret = -EINVAL;
2049                 goto done;
2050         }
2051
2052         dev_dbg(dev, "enable streams %u:%#llx\n", pad, streams_mask);
2053
2054         /* Call the .enable_streams() operation. */
2055         ret = v4l2_subdev_call(sd, pad, enable_streams, state, pad,
2056                                streams_mask);
2057         if (ret) {
2058                 dev_dbg(dev, "enable streams %u:%#llx failed: %d\n", pad,
2059                         streams_mask, ret);
2060                 goto done;
2061         }
2062
2063         /* Mark the streams as enabled. */
2064         for (i = 0; i < state->stream_configs.num_configs; ++i) {
2065                 struct v4l2_subdev_stream_config *cfg =
2066                         &state->stream_configs.configs[i];
2067
2068                 if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
2069                         cfg->enabled = true;
2070         }
2071
2072 done:
2073         v4l2_subdev_unlock_state(state);
2074
2075         return ret;
2076 }
2077 EXPORT_SYMBOL_GPL(v4l2_subdev_enable_streams);
2078
2079 static int v4l2_subdev_disable_streams_fallback(struct v4l2_subdev *sd, u32 pad,
2080                                                 u64 streams_mask)
2081 {
2082         struct device *dev = sd->entity.graph_obj.mdev->dev;
2083         unsigned int i;
2084         int ret;
2085
2086         /*
2087          * If the subdev doesn't implement pad-based stream enable, fall  back
2088          * on the .s_stream() operation. This can only be done for subdevs that
2089          * have a single source pad, as sd->enabled_streams is global to the
2090          * subdev.
2091          */
2092         if (!(sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE))
2093                 return -EOPNOTSUPP;
2094
2095         for (i = 0; i < sd->entity.num_pads; ++i) {
2096                 if (i != pad && sd->entity.pads[i].flags & MEDIA_PAD_FL_SOURCE)
2097                         return -EOPNOTSUPP;
2098         }
2099
2100         if ((sd->enabled_streams & streams_mask) != streams_mask) {
2101                 dev_dbg(dev, "set of streams %#llx already disabled on %s:%u\n",
2102                         streams_mask, sd->entity.name, pad);
2103                 return -EALREADY;
2104         }
2105
2106         /* Stop streaming when the last streams are disabled. */
2107         if (!(sd->enabled_streams & ~streams_mask)) {
2108                 ret = v4l2_subdev_call(sd, video, s_stream, 0);
2109                 if (ret)
2110                         return ret;
2111         }
2112
2113         sd->enabled_streams &= ~streams_mask;
2114
2115         return 0;
2116 }
2117
2118 int v4l2_subdev_disable_streams(struct v4l2_subdev *sd, u32 pad,
2119                                 u64 streams_mask)
2120 {
2121         struct device *dev = sd->entity.graph_obj.mdev->dev;
2122         struct v4l2_subdev_state *state;
2123         u64 found_streams = 0;
2124         unsigned int i;
2125         int ret;
2126
2127         /* A few basic sanity checks first. */
2128         if (pad >= sd->entity.num_pads)
2129                 return -EINVAL;
2130
2131         if (!streams_mask)
2132                 return 0;
2133
2134         /* Fallback on .s_stream() if .disable_streams() isn't available. */
2135         if (!sd->ops->pad || !sd->ops->pad->disable_streams)
2136                 return v4l2_subdev_disable_streams_fallback(sd, pad,
2137                                                             streams_mask);
2138
2139         state = v4l2_subdev_lock_and_get_active_state(sd);
2140
2141         /*
2142          * Verify that the requested streams exist and that they are not
2143          * already disabled.
2144          */
2145         for (i = 0; i < state->stream_configs.num_configs; ++i) {
2146                 struct v4l2_subdev_stream_config *cfg =
2147                         &state->stream_configs.configs[i];
2148
2149                 if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
2150                         continue;
2151
2152                 found_streams |= BIT_ULL(cfg->stream);
2153
2154                 if (!cfg->enabled) {
2155                         dev_dbg(dev, "stream %u already disabled on %s:%u\n",
2156                                 cfg->stream, sd->entity.name, pad);
2157                         ret = -EALREADY;
2158                         goto done;
2159                 }
2160         }
2161
2162         if (found_streams != streams_mask) {
2163                 dev_dbg(dev, "streams 0x%llx not found on %s:%u\n",
2164                         streams_mask & ~found_streams, sd->entity.name, pad);
2165                 ret = -EINVAL;
2166                 goto done;
2167         }
2168
2169         dev_dbg(dev, "disable streams %u:%#llx\n", pad, streams_mask);
2170
2171         /* Call the .disable_streams() operation. */
2172         ret = v4l2_subdev_call(sd, pad, disable_streams, state, pad,
2173                                streams_mask);
2174         if (ret) {
2175                 dev_dbg(dev, "disable streams %u:%#llx failed: %d\n", pad,
2176                         streams_mask, ret);
2177                 goto done;
2178         }
2179
2180         /* Mark the streams as disabled. */
2181         for (i = 0; i < state->stream_configs.num_configs; ++i) {
2182                 struct v4l2_subdev_stream_config *cfg =
2183                         &state->stream_configs.configs[i];
2184
2185                 if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
2186                         cfg->enabled = false;
2187         }
2188
2189 done:
2190         v4l2_subdev_unlock_state(state);
2191
2192         return ret;
2193 }
2194 EXPORT_SYMBOL_GPL(v4l2_subdev_disable_streams);
2195
2196 int v4l2_subdev_s_stream_helper(struct v4l2_subdev *sd, int enable)
2197 {
2198         struct v4l2_subdev_state *state;
2199         struct v4l2_subdev_route *route;
2200         struct media_pad *pad;
2201         u64 source_mask = 0;
2202         int pad_index = -1;
2203
2204         /*
2205          * Find the source pad. This helper is meant for subdevs that have a
2206          * single source pad, so failures shouldn't happen, but catch them
2207          * loudly nonetheless as they indicate a driver bug.
2208          */
2209         media_entity_for_each_pad(&sd->entity, pad) {
2210                 if (pad->flags & MEDIA_PAD_FL_SOURCE) {
2211                         pad_index = pad->index;
2212                         break;
2213                 }
2214         }
2215
2216         if (WARN_ON(pad_index == -1))
2217                 return -EINVAL;
2218
2219         /*
2220          * As there's a single source pad, just collect all the source streams.
2221          */
2222         state = v4l2_subdev_lock_and_get_active_state(sd);
2223
2224         for_each_active_route(&state->routing, route)
2225                 source_mask |= BIT_ULL(route->source_stream);
2226
2227         v4l2_subdev_unlock_state(state);
2228
2229         if (enable)
2230                 return v4l2_subdev_enable_streams(sd, pad_index, source_mask);
2231         else
2232                 return v4l2_subdev_disable_streams(sd, pad_index, source_mask);
2233 }
2234 EXPORT_SYMBOL_GPL(v4l2_subdev_s_stream_helper);
2235
2236 #endif /* CONFIG_VIDEO_V4L2_SUBDEV_API */
2237
2238 #endif /* CONFIG_MEDIA_CONTROLLER */
2239
2240 void v4l2_subdev_init(struct v4l2_subdev *sd, const struct v4l2_subdev_ops *ops)
2241 {
2242         INIT_LIST_HEAD(&sd->list);
2243         BUG_ON(!ops);
2244         sd->ops = ops;
2245         sd->v4l2_dev = NULL;
2246         sd->flags = 0;
2247         sd->name[0] = '\0';
2248         sd->grp_id = 0;
2249         sd->dev_priv = NULL;
2250         sd->host_priv = NULL;
2251         sd->privacy_led = NULL;
2252         INIT_LIST_HEAD(&sd->async_subdev_endpoint_list);
2253 #if defined(CONFIG_MEDIA_CONTROLLER)
2254         sd->entity.name = sd->name;
2255         sd->entity.obj_type = MEDIA_ENTITY_TYPE_V4L2_SUBDEV;
2256         sd->entity.function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
2257 #endif
2258 }
2259 EXPORT_SYMBOL(v4l2_subdev_init);
2260
2261 void v4l2_subdev_notify_event(struct v4l2_subdev *sd,
2262                               const struct v4l2_event *ev)
2263 {
2264         v4l2_event_queue(sd->devnode, ev);
2265         v4l2_subdev_notify(sd, V4L2_DEVICE_NOTIFY_EVENT, (void *)ev);
2266 }
2267 EXPORT_SYMBOL_GPL(v4l2_subdev_notify_event);
2268
2269 int v4l2_subdev_get_privacy_led(struct v4l2_subdev *sd)
2270 {
2271 #if IS_REACHABLE(CONFIG_LEDS_CLASS)
2272         sd->privacy_led = led_get(sd->dev, "privacy-led");
2273         if (IS_ERR(sd->privacy_led) && PTR_ERR(sd->privacy_led) != -ENOENT)
2274                 return dev_err_probe(sd->dev, PTR_ERR(sd->privacy_led),
2275                                      "getting privacy LED\n");
2276
2277         if (!IS_ERR_OR_NULL(sd->privacy_led)) {
2278                 mutex_lock(&sd->privacy_led->led_access);
2279                 led_sysfs_disable(sd->privacy_led);
2280                 led_trigger_remove(sd->privacy_led);
2281                 led_set_brightness(sd->privacy_led, 0);
2282                 mutex_unlock(&sd->privacy_led->led_access);
2283         }
2284 #endif
2285         return 0;
2286 }
2287 EXPORT_SYMBOL_GPL(v4l2_subdev_get_privacy_led);
2288
2289 void v4l2_subdev_put_privacy_led(struct v4l2_subdev *sd)
2290 {
2291 #if IS_REACHABLE(CONFIG_LEDS_CLASS)
2292         if (!IS_ERR_OR_NULL(sd->privacy_led)) {
2293                 mutex_lock(&sd->privacy_led->led_access);
2294                 led_sysfs_enable(sd->privacy_led);
2295                 mutex_unlock(&sd->privacy_led->led_access);
2296                 led_put(sd->privacy_led);
2297         }
2298 #endif
2299 }
2300 EXPORT_SYMBOL_GPL(v4l2_subdev_put_privacy_led);