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