drivers/media/platform/nxp/imx8-isi/imx8-isi-crossbar.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * i.MX8 ISI - Input crossbar switch
   4  *
   5  * Copyright (c) 2022 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
   6  */
   7
   8 #include <linux/device.h>
   9 #include <linux/errno.h>
  10 #include <linux/kernel.h>
  11 #include <linux/minmax.h>
  12 #include <linux/regmap.h>
  13 #include <linux/slab.h>
  14 #include <linux/string.h>
  15 #include <linux/types.h>
  16
  17 #include <media/media-entity.h>
  18 #include <media/v4l2-subdev.h>
  19
  20 #include "imx8-isi-core.h"
  21
  22 static inline struct mxc_isi_crossbar *to_isi_crossbar(struct v4l2_subdev *sd)
  23 {
  24         return container_of(sd, struct mxc_isi_crossbar, sd);
  25 }
  26
  27 static int mxc_isi_crossbar_gasket_enable(struct mxc_isi_crossbar *xbar,
  28                                           struct v4l2_subdev_state *state,
  29                                           struct v4l2_subdev *remote_sd,
  30                                           u32 remote_pad, unsigned int port)
  31 {
  32         struct mxc_isi_dev *isi = xbar->isi;
  33         const struct mxc_gasket_ops *gasket_ops = isi->pdata->gasket_ops;
  34         const struct v4l2_mbus_framefmt *fmt;
  35         struct v4l2_mbus_frame_desc fd;
  36         int ret;
  37
  38         if (!gasket_ops)
  39                 return 0;
  40
  41         /*
  42          * Configure and enable the gasket with the frame size and CSI-2 data
  43          * type. For YUV422 8-bit, enable dual component mode unconditionally,
  44          * to match the configuration of the CSIS.
  45          */
  46
  47         ret = v4l2_subdev_call(remote_sd, pad, get_frame_desc, remote_pad, &fd);
  48         if (ret) {
  49                 dev_err(isi->dev,
  50                         "failed to get frame descriptor from '%s':%u: %d\n",
  51                         remote_sd->name, remote_pad, ret);
  52                 return ret;
  53         }
  54
  55         if (fd.num_entries != 1) {
  56                 dev_err(isi->dev, "invalid frame descriptor for '%s':%u\n",
  57                         remote_sd->name, remote_pad);
  58                 return -EINVAL;
  59         }
  60
  61         fmt = v4l2_subdev_state_get_format(state, port, 0);
  62         if (!fmt)
  63                 return -EINVAL;
  64
  65         gasket_ops->enable(isi, &fd, fmt, port);
  66         return 0;
  67 }
  68
  69 static void mxc_isi_crossbar_gasket_disable(struct mxc_isi_crossbar *xbar,
  70                                             unsigned int port)
  71 {
  72         struct mxc_isi_dev *isi = xbar->isi;
  73         const struct mxc_gasket_ops *gasket_ops = isi->pdata->gasket_ops;
  74
  75         if (!gasket_ops)
  76                 return;
  77
  78         gasket_ops->disable(isi, port);
  79 }
  80
  81 /* -----------------------------------------------------------------------------
  82  * V4L2 subdev operations
  83  */
  84
  85 static const struct v4l2_mbus_framefmt mxc_isi_crossbar_default_format = {
  86         .code = MXC_ISI_DEF_MBUS_CODE_SINK,
  87         .width = MXC_ISI_DEF_WIDTH,
  88         .height = MXC_ISI_DEF_HEIGHT,
  89         .field = V4L2_FIELD_NONE,
  90         .colorspace = MXC_ISI_DEF_COLOR_SPACE,
  91         .ycbcr_enc = MXC_ISI_DEF_YCBCR_ENC,
  92         .quantization = MXC_ISI_DEF_QUANTIZATION,
  93         .xfer_func = MXC_ISI_DEF_XFER_FUNC,
  94 };
  95
  96 static int __mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
  97                                           struct v4l2_subdev_state *state,
  98                                           struct v4l2_subdev_krouting *routing)
  99 {
 100         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 101         struct v4l2_subdev_route *route;
 102         int ret;
 103
 104         ret = v4l2_subdev_routing_validate(sd, routing,
 105                                            V4L2_SUBDEV_ROUTING_NO_N_TO_1);
 106         if (ret)
 107                 return ret;
 108
 109         /* The memory input can be routed to the first pipeline only. */
 110         for_each_active_route(&state->routing, route) {
 111                 if (route->sink_pad == xbar->num_sinks - 1 &&
 112                     route->source_pad != xbar->num_sinks) {
 113                         dev_dbg(xbar->isi->dev,
 114                                 "invalid route from memory input (%u) to pipe %u\n",
 115                                 route->sink_pad,
 116                                 route->source_pad - xbar->num_sinks);
 117                         return -EINVAL;
 118                 }
 119         }
 120
 121         return v4l2_subdev_set_routing_with_fmt(sd, state, routing,
 122                                                 &mxc_isi_crossbar_default_format);
 123 }
 124
 125 static struct v4l2_subdev *
 126 mxc_isi_crossbar_xlate_streams(struct mxc_isi_crossbar *xbar,
 127                                struct v4l2_subdev_state *state,
 128                                u32 source_pad, u64 source_streams,
 129                                u32 *__sink_pad, u64 *__sink_streams,
 130                                u32 *remote_pad)
 131 {
 132         struct v4l2_subdev_route *route;
 133         struct v4l2_subdev *sd;
 134         struct media_pad *pad;
 135         u64 sink_streams = 0;
 136         int sink_pad = -1;
 137
 138         /*
 139          * Translate the source pad and streams to the sink side. The routing
 140          * validation forbids stream merging, so all matching entries in the
 141          * routing table are guaranteed to have the same sink pad.
 142          *
 143          * TODO: This is likely worth a helper function, it could perhaps be
 144          * supported by v4l2_subdev_state_xlate_streams() with pad1 set to -1.
 145          */
 146         for_each_active_route(&state->routing, route) {
 147                 if (route->source_pad != source_pad ||
 148                     !(source_streams & BIT(route->source_stream)))
 149                         continue;
 150
 151                 sink_streams |= BIT(route->sink_stream);
 152                 sink_pad = route->sink_pad;
 153         }
 154
 155         if (sink_pad < 0) {
 156                 dev_dbg(xbar->isi->dev,
 157                         "no stream connected to pipeline %u\n",
 158                         source_pad - xbar->num_sinks);
 159                 return ERR_PTR(-EPIPE);
 160         }
 161
 162         pad = media_pad_remote_pad_first(&xbar->pads[sink_pad]);
 163         sd = media_entity_to_v4l2_subdev(pad->entity);
 164         if (!sd) {
 165                 dev_dbg(xbar->isi->dev,
 166                         "no entity connected to crossbar input %u\n",
 167                         sink_pad);
 168                 return ERR_PTR(-EPIPE);
 169         }
 170
 171         *__sink_pad = sink_pad;
 172         *__sink_streams = sink_streams;
 173         *remote_pad = pad->index;
 174
 175         return sd;
 176 }
 177
 178 static int mxc_isi_crossbar_init_state(struct v4l2_subdev *sd,
 179                                        struct v4l2_subdev_state *state)
 180 {
 181         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 182         struct v4l2_subdev_krouting routing = { };
 183         struct v4l2_subdev_route *routes;
 184         unsigned int i;
 185         int ret;
 186
 187         /*
 188          * Create a 1:1 mapping between pixel link inputs and outputs to
 189          * pipelines by default.
 190          */
 191         routes = kcalloc(xbar->num_sources, sizeof(*routes), GFP_KERNEL);
 192         if (!routes)
 193                 return -ENOMEM;
 194
 195         for (i = 0; i < xbar->num_sources; ++i) {
 196                 struct v4l2_subdev_route *route = &routes[i];
 197
 198                 route->sink_pad = i;
 199                 route->source_pad = i + xbar->num_sinks;
 200                 route->flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE;
 201         }
 202
 203         routing.num_routes = xbar->num_sources;
 204         routing.routes = routes;
 205
 206         ret = __mxc_isi_crossbar_set_routing(sd, state, &routing);
 207
 208         kfree(routes);
 209
 210         return ret;
 211 }
 212
 213 static int mxc_isi_crossbar_enum_mbus_code(struct v4l2_subdev *sd,
 214                                            struct v4l2_subdev_state *state,
 215                                            struct v4l2_subdev_mbus_code_enum *code)
 216 {
 217         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 218         const struct mxc_isi_bus_format_info *info;
 219
 220         if (code->pad >= xbar->num_sinks) {
 221                 const struct v4l2_mbus_framefmt *format;
 222
 223                 /*
 224                  * The media bus code on source pads is identical to the
 225                  * connected sink pad.
 226                  */
 227                 if (code->index > 0)
 228                         return -EINVAL;
 229
 230                 format = v4l2_subdev_state_get_opposite_stream_format(state,
 231                                                                       code->pad,
 232                                                                       code->stream);
 233                 if (!format)
 234                         return -EINVAL;
 235
 236                 code->code = format->code;
 237
 238                 return 0;
 239         }
 240
 241         info = mxc_isi_bus_format_by_index(code->index, MXC_ISI_PIPE_PAD_SINK);
 242         if (!info)
 243                 return -EINVAL;
 244
 245         code->code = info->mbus_code;
 246
 247         return 0;
 248 }
 249
 250 static int mxc_isi_crossbar_set_fmt(struct v4l2_subdev *sd,
 251                                     struct v4l2_subdev_state *state,
 252                                     struct v4l2_subdev_format *fmt)
 253 {
 254         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 255         struct v4l2_mbus_framefmt *sink_fmt;
 256         struct v4l2_subdev_route *route;
 257
 258         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
 259             media_pad_is_streaming(&xbar->pads[fmt->pad]))
 260                 return -EBUSY;
 261
 262         /*
 263          * The source pad format is always identical to the sink pad format and
 264          * can't be modified.
 265          */
 266         if (fmt->pad >= xbar->num_sinks)
 267                 return v4l2_subdev_get_fmt(sd, state, fmt);
 268
 269         /* Validate the requested format. */
 270         if (!mxc_isi_bus_format_by_code(fmt->format.code, MXC_ISI_PIPE_PAD_SINK))
 271                 fmt->format.code = MXC_ISI_DEF_MBUS_CODE_SINK;
 272
 273         fmt->format.width = clamp_t(unsigned int, fmt->format.width,
 274                                     MXC_ISI_MIN_WIDTH, MXC_ISI_MAX_WIDTH_CHAINED);
 275         fmt->format.height = clamp_t(unsigned int, fmt->format.height,
 276                                      MXC_ISI_MIN_HEIGHT, MXC_ISI_MAX_HEIGHT);
 277         fmt->format.field = V4L2_FIELD_NONE;
 278
 279         /*
 280          * Set the format on the sink stream and propagate it to the source
 281          * streams.
 282          */
 283         sink_fmt = v4l2_subdev_state_get_format(state, fmt->pad, fmt->stream);
 284         if (!sink_fmt)
 285                 return -EINVAL;
 286
 287         *sink_fmt = fmt->format;
 288
 289         /* TODO: A format propagation helper would be useful. */
 290         for_each_active_route(&state->routing, route) {
 291                 struct v4l2_mbus_framefmt *source_fmt;
 292
 293                 if (route->sink_pad != fmt->pad ||
 294                     route->sink_stream != fmt->stream)
 295                         continue;
 296
 297                 source_fmt = v4l2_subdev_state_get_format(state,
 298                                                           route->source_pad,
 299                                                           route->source_stream);
 300                 if (!source_fmt)
 301                         return -EINVAL;
 302
 303                 *source_fmt = fmt->format;
 304         }
 305
 306         return 0;
 307 }
 308
 309 static int mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
 310                                         struct v4l2_subdev_state *state,
 311                                         enum v4l2_subdev_format_whence which,
 312                                         struct v4l2_subdev_krouting *routing)
 313 {
 314         if (which == V4L2_SUBDEV_FORMAT_ACTIVE &&
 315             media_entity_is_streaming(&sd->entity))
 316                 return -EBUSY;
 317
 318         return __mxc_isi_crossbar_set_routing(sd, state, routing);
 319 }
 320
 321 static int mxc_isi_crossbar_enable_streams(struct v4l2_subdev *sd,
 322                                            struct v4l2_subdev_state *state,
 323                                            u32 pad, u64 streams_mask)
 324 {
 325         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 326         struct v4l2_subdev *remote_sd;
 327         struct mxc_isi_input *input;
 328         u64 sink_streams;
 329         u32 sink_pad;
 330         u32 remote_pad;
 331         int ret;
 332
 333         remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
 334                                                    &sink_pad, &sink_streams,
 335                                                    &remote_pad);
 336         if (IS_ERR(remote_sd))
 337                 return PTR_ERR(remote_sd);
 338
 339         input = &xbar->inputs[sink_pad];
 340
 341         /*
 342          * TODO: Track per-stream enable counts to support multiplexed
 343          * streams.
 344          */
 345         if (!input->enable_count) {
 346                 ret = mxc_isi_crossbar_gasket_enable(xbar, state, remote_sd,
 347                                                      remote_pad, sink_pad);
 348                 if (ret)
 349                         return ret;
 350
 351                 ret = v4l2_subdev_enable_streams(remote_sd, remote_pad,
 352                                                  sink_streams);
 353                 if (ret) {
 354                         dev_err(xbar->isi->dev,
 355                                 "failed to %s streams 0x%llx on '%s':%u: %d\n",
 356                                 "enable", sink_streams, remote_sd->name,
 357                                 remote_pad, ret);
 358                         mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
 359                         return ret;
 360                 }
 361         }
 362
 363         input->enable_count++;
 364
 365         return 0;
 366 }
 367
 368 static int mxc_isi_crossbar_disable_streams(struct v4l2_subdev *sd,
 369                                             struct v4l2_subdev_state *state,
 370                                             u32 pad, u64 streams_mask)
 371 {
 372         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 373         struct v4l2_subdev *remote_sd;
 374         struct mxc_isi_input *input;
 375         u64 sink_streams;
 376         u32 sink_pad;
 377         u32 remote_pad;
 378         int ret = 0;
 379
 380         remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
 381                                                    &sink_pad, &sink_streams,
 382                                                    &remote_pad);
 383         if (IS_ERR(remote_sd))
 384                 return PTR_ERR(remote_sd);
 385
 386         input = &xbar->inputs[sink_pad];
 387
 388         input->enable_count--;
 389
 390         if (!input->enable_count) {
 391                 ret = v4l2_subdev_disable_streams(remote_sd, remote_pad,
 392                                                   sink_streams);
 393                 if (ret)
 394                         dev_err(xbar->isi->dev,
 395                                 "failed to %s streams 0x%llx on '%s':%u: %d\n",
 396                                 "disable", sink_streams, remote_sd->name,
 397                                 remote_pad, ret);
 398
 399                 mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
 400         }
 401
 402         return ret;
 403 }
 404
 405 static const struct v4l2_subdev_pad_ops mxc_isi_crossbar_subdev_pad_ops = {
 406         .enum_mbus_code = mxc_isi_crossbar_enum_mbus_code,
 407         .get_fmt = v4l2_subdev_get_fmt,
 408         .set_fmt = mxc_isi_crossbar_set_fmt,
 409         .set_routing = mxc_isi_crossbar_set_routing,
 410         .enable_streams = mxc_isi_crossbar_enable_streams,
 411         .disable_streams = mxc_isi_crossbar_disable_streams,
 412 };
 413
 414 static const struct v4l2_subdev_ops mxc_isi_crossbar_subdev_ops = {
 415         .pad = &mxc_isi_crossbar_subdev_pad_ops,
 416 };
 417
 418 static const struct v4l2_subdev_internal_ops mxc_isi_crossbar_internal_ops = {
 419         .init_state = mxc_isi_crossbar_init_state,
 420 };
 421
 422 static const struct media_entity_operations mxc_isi_cross_entity_ops = {
 423         .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
 424         .link_validate  = v4l2_subdev_link_validate,
 425         .has_pad_interdep = v4l2_subdev_has_pad_interdep,
 426 };
 427
 428 /* -----------------------------------------------------------------------------
 429  * Init & cleanup
 430  */
 431
 432 int mxc_isi_crossbar_init(struct mxc_isi_dev *isi)
 433 {
 434         struct mxc_isi_crossbar *xbar = &isi->crossbar;
 435         struct v4l2_subdev *sd = &xbar->sd;
 436         unsigned int num_pads;
 437         unsigned int i;
 438         int ret;
 439
 440         xbar->isi = isi;
 441
 442         v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops);
 443         sd->internal_ops = &mxc_isi_crossbar_internal_ops;
 444         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
 445         strscpy(sd->name, "crossbar", sizeof(sd->name));
 446         sd->dev = isi->dev;
 447
 448         sd->entity.function = MEDIA_ENT_F_VID_MUX;
 449         sd->entity.ops = &mxc_isi_cross_entity_ops;
 450
 451         /*
 452          * The subdev has one sink and one source per port, plus one sink for
 453          * the memory input.
 454          */
 455         xbar->num_sinks = isi->pdata->num_ports + 1;
 456         xbar->num_sources = isi->pdata->num_ports;
 457         num_pads = xbar->num_sinks + xbar->num_sources;
 458
 459         xbar->pads = kcalloc(num_pads, sizeof(*xbar->pads), GFP_KERNEL);
 460         if (!xbar->pads)
 461                 return -ENOMEM;
 462
 463         xbar->inputs = kcalloc(xbar->num_sinks, sizeof(*xbar->inputs),
 464                                GFP_KERNEL);
 465         if (!xbar->inputs) {
 466                 ret = -ENOMEM;
 467                 goto err_free;
 468         }
 469
 470         for (i = 0; i < xbar->num_sinks; ++i)
 471                 xbar->pads[i].flags = MEDIA_PAD_FL_SINK
 472                                     | MEDIA_PAD_FL_MUST_CONNECT;
 473         for (i = 0; i < xbar->num_sources; ++i)
 474                 xbar->pads[i + xbar->num_sinks].flags = MEDIA_PAD_FL_SOURCE;
 475
 476         ret = media_entity_pads_init(&sd->entity, num_pads, xbar->pads);
 477         if (ret)
 478                 goto err_free;
 479
 480         ret = v4l2_subdev_init_finalize(sd);
 481         if (ret < 0)
 482                 goto err_entity;
 483
 484         return 0;
 485
 486 err_entity:
 487         media_entity_cleanup(&sd->entity);
 488 err_free:
 489         kfree(xbar->pads);
 490         kfree(xbar->inputs);
 491
 492         return ret;
 493 }
 494
 495 void mxc_isi_crossbar_cleanup(struct mxc_isi_crossbar *xbar)
 496 {
 497         media_entity_cleanup(&xbar->sd.entity);
 498         kfree(xbar->pads);
 499         kfree(xbar->inputs);
 500 }
 501
 502 int mxc_isi_crossbar_register(struct mxc_isi_crossbar *xbar)
 503 {
 504         return v4l2_device_register_subdev(&xbar->isi->v4l2_dev, &xbar->sd);
 505 }
 506
 507 void mxc_isi_crossbar_unregister(struct mxc_isi_crossbar *xbar)
 508 {
 509 }