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
 165         if (!sd) {
 166                 dev_dbg(xbar->isi->dev,
 167                         "no entity connected to crossbar input %u\n",
 168                         sink_pad);
 169                 return ERR_PTR(-EPIPE);
 170         }
 171
 172         *__sink_pad = sink_pad;
 173         *__sink_streams = sink_streams;
 174         *remote_pad = pad->index;
 175
 176         return sd;
 177 }
 178
 179 static int mxc_isi_crossbar_init_state(struct v4l2_subdev *sd,
 180                                        struct v4l2_subdev_state *state)
 181 {
 182         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 183         struct v4l2_subdev_krouting routing = { };
 184         struct v4l2_subdev_route *routes;
 185         unsigned int i;
 186         int ret;
 187
 188         /*
 189          * Create a 1:1 mapping between pixel link inputs and outputs to
 190          * pipelines by default.
 191          */
 192         routes = kcalloc(xbar->num_sources, sizeof(*routes), GFP_KERNEL);
 193         if (!routes)
 194                 return -ENOMEM;
 195
 196         for (i = 0; i < xbar->num_sources; ++i) {
 197                 struct v4l2_subdev_route *route = &routes[i];
 198
 199                 route->sink_pad = i;
 200                 route->source_pad = i + xbar->num_sinks;
 201                 route->flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE;
 202         }
 203
 204         routing.num_routes = xbar->num_sources;
 205         routing.routes = routes;
 206
 207         ret = __mxc_isi_crossbar_set_routing(sd, state, &routing);
 208
 209         kfree(routes);
 210
 211         return ret;
 212 }
 213
 214 static int mxc_isi_crossbar_enum_mbus_code(struct v4l2_subdev *sd,
 215                                            struct v4l2_subdev_state *state,
 216                                            struct v4l2_subdev_mbus_code_enum *code)
 217 {
 218         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 219         const struct mxc_isi_bus_format_info *info;
 220
 221         if (code->pad >= xbar->num_sinks) {
 222                 const struct v4l2_mbus_framefmt *format;
 223
 224                 /*
 225                  * The media bus code on source pads is identical to the
 226                  * connected sink pad.
 227                  */
 228                 if (code->index > 0)
 229                         return -EINVAL;
 230
 231                 format = v4l2_subdev_state_get_opposite_stream_format(state,
 232                                                                       code->pad,
 233                                                                       code->stream);
 234                 if (!format)
 235                         return -EINVAL;
 236
 237                 code->code = format->code;
 238
 239                 return 0;
 240         }
 241
 242         info = mxc_isi_bus_format_by_index(code->index, MXC_ISI_PIPE_PAD_SINK);
 243         if (!info)
 244                 return -EINVAL;
 245
 246         code->code = info->mbus_code;
 247
 248         return 0;
 249 }
 250
 251 static int mxc_isi_crossbar_set_fmt(struct v4l2_subdev *sd,
 252                                     struct v4l2_subdev_state *state,
 253                                     struct v4l2_subdev_format *fmt)
 254 {
 255         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 256         struct v4l2_mbus_framefmt *sink_fmt;
 257         struct v4l2_subdev_route *route;
 258
 259         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
 260             media_pad_is_streaming(&xbar->pads[fmt->pad]))
 261                 return -EBUSY;
 262
 263         /*
 264          * The source pad format is always identical to the sink pad format and
 265          * can't be modified.
 266          */
 267         if (fmt->pad >= xbar->num_sinks)
 268                 return v4l2_subdev_get_fmt(sd, state, fmt);
 269
 270         /* Validate the requested format. */
 271         if (!mxc_isi_bus_format_by_code(fmt->format.code, MXC_ISI_PIPE_PAD_SINK))
 272                 fmt->format.code = MXC_ISI_DEF_MBUS_CODE_SINK;
 273
 274         fmt->format.width = clamp_t(unsigned int, fmt->format.width,
 275                                     MXC_ISI_MIN_WIDTH, MXC_ISI_MAX_WIDTH_CHAINED);
 276         fmt->format.height = clamp_t(unsigned int, fmt->format.height,
 277                                      MXC_ISI_MIN_HEIGHT, MXC_ISI_MAX_HEIGHT);
 278         fmt->format.field = V4L2_FIELD_NONE;
 279
 280         /*
 281          * Set the format on the sink stream and propagate it to the source
 282          * streams.
 283          */
 284         sink_fmt = v4l2_subdev_state_get_format(state, fmt->pad, fmt->stream);
 285         if (!sink_fmt)
 286                 return -EINVAL;
 287
 288         *sink_fmt = fmt->format;
 289
 290         /* TODO: A format propagation helper would be useful. */
 291         for_each_active_route(&state->routing, route) {
 292                 struct v4l2_mbus_framefmt *source_fmt;
 293
 294                 if (route->sink_pad != fmt->pad ||
 295                     route->sink_stream != fmt->stream)
 296                         continue;
 297
 298                 source_fmt = v4l2_subdev_state_get_format(state,
 299                                                           route->source_pad,
 300                                                           route->source_stream);
 301                 if (!source_fmt)
 302                         return -EINVAL;
 303
 304                 *source_fmt = fmt->format;
 305         }
 306
 307         return 0;
 308 }
 309
 310 static int mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
 311                                         struct v4l2_subdev_state *state,
 312                                         enum v4l2_subdev_format_whence which,
 313                                         struct v4l2_subdev_krouting *routing)
 314 {
 315         if (which == V4L2_SUBDEV_FORMAT_ACTIVE &&
 316             media_entity_is_streaming(&sd->entity))
 317                 return -EBUSY;
 318
 319         return __mxc_isi_crossbar_set_routing(sd, state, routing);
 320 }
 321
 322 static int mxc_isi_crossbar_enable_streams(struct v4l2_subdev *sd,
 323                                            struct v4l2_subdev_state *state,
 324                                            u32 pad, u64 streams_mask)
 325 {
 326         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 327         struct v4l2_subdev *remote_sd;
 328         struct mxc_isi_input *input;
 329         u64 sink_streams;
 330         u32 sink_pad;
 331         u32 remote_pad;
 332         int ret;
 333
 334         remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
 335                                                    &sink_pad, &sink_streams,
 336                                                    &remote_pad);
 337         if (IS_ERR(remote_sd))
 338                 return PTR_ERR(remote_sd);
 339
 340         input = &xbar->inputs[sink_pad];
 341
 342         /*
 343          * TODO: Track per-stream enable counts to support multiplexed
 344          * streams.
 345          */
 346         if (!input->enable_count) {
 347                 ret = mxc_isi_crossbar_gasket_enable(xbar, state, remote_sd,
 348                                                      remote_pad, sink_pad);
 349                 if (ret)
 350                         return ret;
 351
 352                 ret = v4l2_subdev_enable_streams(remote_sd, remote_pad,
 353                                                  sink_streams);
 354                 if (ret) {
 355                         dev_err(xbar->isi->dev,
 356                                 "failed to %s streams 0x%llx on '%s':%u: %d\n",
 357                                 "enable", sink_streams, remote_sd->name,
 358                                 remote_pad, ret);
 359                         mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
 360                         return ret;
 361                 }
 362         }
 363
 364         input->enable_count++;
 365
 366         return 0;
 367 }
 368
 369 static int mxc_isi_crossbar_disable_streams(struct v4l2_subdev *sd,
 370                                             struct v4l2_subdev_state *state,
 371                                             u32 pad, u64 streams_mask)
 372 {
 373         struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
 374         struct v4l2_subdev *remote_sd;
 375         struct mxc_isi_input *input;
 376         u64 sink_streams;
 377         u32 sink_pad;
 378         u32 remote_pad;
 379         int ret = 0;
 380
 381         remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
 382                                                    &sink_pad, &sink_streams,
 383                                                    &remote_pad);
 384         if (IS_ERR(remote_sd))
 385                 return PTR_ERR(remote_sd);
 386
 387         input = &xbar->inputs[sink_pad];
 388
 389         input->enable_count--;
 390
 391         if (!input->enable_count) {
 392                 ret = v4l2_subdev_disable_streams(remote_sd, remote_pad,
 393                                                   sink_streams);
 394                 if (ret)
 395                         dev_err(xbar->isi->dev,
 396                                 "failed to %s streams 0x%llx on '%s':%u: %d\n",
 397                                 "disable", sink_streams, remote_sd->name,
 398                                 remote_pad, ret);
 399
 400                 mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
 401         }
 402
 403         return ret;
 404 }
 405
 406 static const struct v4l2_subdev_pad_ops mxc_isi_crossbar_subdev_pad_ops = {
 407         .enum_mbus_code = mxc_isi_crossbar_enum_mbus_code,
 408         .get_fmt = v4l2_subdev_get_fmt,
 409         .set_fmt = mxc_isi_crossbar_set_fmt,
 410         .set_routing = mxc_isi_crossbar_set_routing,
 411         .enable_streams = mxc_isi_crossbar_enable_streams,
 412         .disable_streams = mxc_isi_crossbar_disable_streams,
 413 };
 414
 415 static const struct v4l2_subdev_ops mxc_isi_crossbar_subdev_ops = {
 416         .pad = &mxc_isi_crossbar_subdev_pad_ops,
 417 };
 418
 419 static const struct v4l2_subdev_internal_ops mxc_isi_crossbar_internal_ops = {
 420         .init_state = mxc_isi_crossbar_init_state,
 421 };
 422
 423 static const struct media_entity_operations mxc_isi_cross_entity_ops = {
 424         .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
 425         .link_validate  = v4l2_subdev_link_validate,
 426         .has_pad_interdep = v4l2_subdev_has_pad_interdep,
 427 };
 428
 429 /* -----------------------------------------------------------------------------
 430  * Init & cleanup
 431  */
 432
 433 int mxc_isi_crossbar_init(struct mxc_isi_dev *isi)
 434 {
 435         struct mxc_isi_crossbar *xbar = &isi->crossbar;
 436         struct v4l2_subdev *sd = &xbar->sd;
 437         unsigned int num_pads;
 438         unsigned int i;
 439         int ret;
 440
 441         xbar->isi = isi;
 442
 443         v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops);
 444         sd->internal_ops = &mxc_isi_crossbar_internal_ops;
 445         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
 446         strscpy(sd->name, "crossbar", sizeof(sd->name));
 447         sd->dev = isi->dev;
 448
 449         sd->entity.function = MEDIA_ENT_F_VID_MUX;
 450         sd->entity.ops = &mxc_isi_cross_entity_ops;
 451
 452         /*
 453          * The subdev has one sink and one source per port, plus one sink for
 454          * the memory input.
 455          */
 456         xbar->num_sinks = isi->pdata->num_ports + 1;
 457         xbar->num_sources = isi->pdata->num_ports;
 458         num_pads = xbar->num_sinks + xbar->num_sources;
 459
 460         xbar->pads = kcalloc(num_pads, sizeof(*xbar->pads), GFP_KERNEL);
 461         if (!xbar->pads)
 462                 return -ENOMEM;
 463
 464         xbar->inputs = kcalloc(xbar->num_sinks, sizeof(*xbar->inputs),
 465                                GFP_KERNEL);
 466         if (!xbar->inputs) {
 467                 ret = -ENOMEM;
 468                 goto err_free;
 469         }
 470
 471         for (i = 0; i < xbar->num_sinks; ++i)
 472                 xbar->pads[i].flags = MEDIA_PAD_FL_SINK;
 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 }