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_stream_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_cfg(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_stream_format(state, fmt->pad, 284 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_stream_format(state, 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 .init_cfg = mxc_isi_crossbar_init_cfg, 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 media_entity_operations mxc_isi_cross_entity_ops = { 420 .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1, 421 .link_validate = v4l2_subdev_link_validate, 422 .has_pad_interdep = v4l2_subdev_has_pad_interdep, 423 }; 424 425 /* ----------------------------------------------------------------------------- 426 * Init & cleanup 427 */ 428 429 int mxc_isi_crossbar_init(struct mxc_isi_dev *isi) 430 { 431 struct mxc_isi_crossbar *xbar = &isi->crossbar; 432 struct v4l2_subdev *sd = &xbar->sd; 433 unsigned int num_pads; 434 unsigned int i; 435 int ret; 436 437 xbar->isi = isi; 438 439 v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops); 440 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS; 441 strscpy(sd->name, "crossbar", sizeof(sd->name)); 442 sd->dev = isi->dev; 443 444 sd->entity.function = MEDIA_ENT_F_VID_MUX; 445 sd->entity.ops = &mxc_isi_cross_entity_ops; 446 447 /* 448 * The subdev has one sink and one source per port, plus one sink for 449 * the memory input. 450 */ 451 xbar->num_sinks = isi->pdata->num_ports + 1; 452 xbar->num_sources = isi->pdata->num_ports; 453 num_pads = xbar->num_sinks + xbar->num_sources; 454 455 xbar->pads = kcalloc(num_pads, sizeof(*xbar->pads), GFP_KERNEL); 456 if (!xbar->pads) 457 return -ENOMEM; 458 459 xbar->inputs = kcalloc(xbar->num_sinks, sizeof(*xbar->inputs), 460 GFP_KERNEL); 461 if (!xbar->inputs) { 462 ret = -ENOMEM; 463 goto err_free; 464 } 465 466 for (i = 0; i < xbar->num_sinks; ++i) 467 xbar->pads[i].flags = MEDIA_PAD_FL_SINK 468 | MEDIA_PAD_FL_MUST_CONNECT; 469 for (i = 0; i < xbar->num_sources; ++i) 470 xbar->pads[i + xbar->num_sinks].flags = MEDIA_PAD_FL_SOURCE; 471 472 ret = media_entity_pads_init(&sd->entity, num_pads, xbar->pads); 473 if (ret) 474 goto err_free; 475 476 ret = v4l2_subdev_init_finalize(sd); 477 if (ret < 0) 478 goto err_entity; 479 480 return 0; 481 482 err_entity: 483 media_entity_cleanup(&sd->entity); 484 err_free: 485 kfree(xbar->pads); 486 kfree(xbar->inputs); 487 488 return ret; 489 } 490 491 void mxc_isi_crossbar_cleanup(struct mxc_isi_crossbar *xbar) 492 { 493 media_entity_cleanup(&xbar->sd.entity); 494 kfree(xbar->pads); 495 kfree(xbar->inputs); 496 } 497 498 int mxc_isi_crossbar_register(struct mxc_isi_crossbar *xbar) 499 { 500 return v4l2_device_register_subdev(&xbar->isi->v4l2_dev, &xbar->sd); 501 } 502 503 void mxc_isi_crossbar_unregister(struct mxc_isi_crossbar *xbar) 504 { 505 } 506