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 
to_isi_crossbar(struct v4l2_subdev * sd)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 
mxc_isi_crossbar_gasket_enable(struct mxc_isi_crossbar * xbar,struct v4l2_subdev_state * state,struct v4l2_subdev * remote_sd,u32 remote_pad,unsigned int port)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 
mxc_isi_crossbar_gasket_disable(struct mxc_isi_crossbar * xbar,unsigned int port)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 
__mxc_isi_crossbar_set_routing(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_krouting * routing)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 *
mxc_isi_crossbar_xlate_streams(struct mxc_isi_crossbar * xbar,struct v4l2_subdev_state * state,u32 source_pad,u64 source_streams,u32 * __sink_pad,u64 * __sink_streams,u32 * remote_pad)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 
mxc_isi_crossbar_init_cfg(struct v4l2_subdev * sd,struct v4l2_subdev_state * state)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 
mxc_isi_crossbar_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_mbus_code_enum * code)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 
mxc_isi_crossbar_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_format * fmt)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 
mxc_isi_crossbar_set_routing(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,enum v4l2_subdev_format_whence which,struct v4l2_subdev_krouting * routing)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 
mxc_isi_crossbar_enable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)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 
mxc_isi_crossbar_disable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)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 
mxc_isi_crossbar_init(struct mxc_isi_dev * isi)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 
mxc_isi_crossbar_cleanup(struct mxc_isi_crossbar * xbar)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 
mxc_isi_crossbar_register(struct mxc_isi_crossbar * xbar)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 
mxc_isi_crossbar_unregister(struct mxc_isi_crossbar * xbar)503 void mxc_isi_crossbar_unregister(struct mxc_isi_crossbar *xbar)
504 {
505 }
506