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 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_cfg(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_stream_format(state, fmt->pad, 285 fmt->stream); 286 if (!sink_fmt) 287 return -EINVAL; 288 289 *sink_fmt = fmt->format; 290 291 /* TODO: A format propagation helper would be useful. */ 292 for_each_active_route(&state->routing, route) { 293 struct v4l2_mbus_framefmt *source_fmt; 294 295 if (route->sink_pad != fmt->pad || 296 route->sink_stream != fmt->stream) 297 continue; 298 299 source_fmt = v4l2_subdev_state_get_stream_format(state, 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 .init_cfg = mxc_isi_crossbar_init_cfg, 408 .enum_mbus_code = mxc_isi_crossbar_enum_mbus_code, 409 .get_fmt = v4l2_subdev_get_fmt, 410 .set_fmt = mxc_isi_crossbar_set_fmt, 411 .set_routing = mxc_isi_crossbar_set_routing, 412 .enable_streams = mxc_isi_crossbar_enable_streams, 413 .disable_streams = mxc_isi_crossbar_disable_streams, 414 }; 415 416 static const struct v4l2_subdev_ops mxc_isi_crossbar_subdev_ops = { 417 .pad = &mxc_isi_crossbar_subdev_pad_ops, 418 }; 419 420 static const struct media_entity_operations mxc_isi_cross_entity_ops = { 421 .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1, 422 .link_validate = v4l2_subdev_link_validate, 423 .has_pad_interdep = v4l2_subdev_has_pad_interdep, 424 }; 425 426 /* ----------------------------------------------------------------------------- 427 * Init & cleanup 428 */ 429 430 int mxc_isi_crossbar_init(struct mxc_isi_dev *isi) 431 { 432 struct mxc_isi_crossbar *xbar = &isi->crossbar; 433 struct v4l2_subdev *sd = &xbar->sd; 434 unsigned int num_pads; 435 unsigned int i; 436 int ret; 437 438 xbar->isi = isi; 439 440 v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops); 441 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS; 442 strscpy(sd->name, "crossbar", sizeof(sd->name)); 443 sd->dev = isi->dev; 444 445 sd->entity.function = MEDIA_ENT_F_VID_MUX; 446 sd->entity.ops = &mxc_isi_cross_entity_ops; 447 448 /* 449 * The subdev has one sink and one source per port, plus one sink for 450 * the memory input. 451 */ 452 xbar->num_sinks = isi->pdata->num_ports + 1; 453 xbar->num_sources = isi->pdata->num_ports; 454 num_pads = xbar->num_sinks + xbar->num_sources; 455 456 xbar->pads = kcalloc(num_pads, sizeof(*xbar->pads), GFP_KERNEL); 457 if (!xbar->pads) 458 return -ENOMEM; 459 460 xbar->inputs = kcalloc(xbar->num_sinks, sizeof(*xbar->inputs), 461 GFP_KERNEL); 462 if (!xbar->inputs) { 463 ret = -ENOMEM; 464 goto err_free; 465 } 466 467 for (i = 0; i < xbar->num_sinks; ++i) 468 xbar->pads[i].flags = MEDIA_PAD_FL_SINK; 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