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